Upgrade to Newton 3.14 and make it a thirdparty lib
This commit is contained in:
Jérôme Leclercq
294
thirdparty/src/newton/dMath/dLinearAlgebra.h
vendored
Normal file
294
thirdparty/src/newton/dMath/dLinearAlgebra.h
vendored
Normal file
@@ -0,0 +1,294 @@
|
||||
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
|
||||
*
|
||||
* This software is provided 'as-is', without any express or implied
|
||||
* warranty. In no event will the authors be held liable for any damages
|
||||
* arising from the use of this software.
|
||||
*
|
||||
* Permission is granted to anyone to use this software for any purpose,
|
||||
* including commercial applications, and to alter it and redistribute it
|
||||
* freely
|
||||
*/
|
||||
|
||||
#include "dStdAfxMath.h"
|
||||
#include "dMathDefines.h"
|
||||
#include "dVector.h"
|
||||
#include "dMatrix.h"
|
||||
#include "dQuaternion.h"
|
||||
|
||||
|
||||
#ifndef __D_LINEAR_ALGEBRA_H__
|
||||
#define __D_LINEAR_ALGEBRA_H__
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma warning (disable: 4100) //unreferenced formal parameter
|
||||
#endif
|
||||
|
||||
|
||||
#define D_MAX_PRAM_INFO_SIZE 16
|
||||
#define D_MAX_PLACEMENT_CONTACTS 128
|
||||
#define D_COMPLEMENTARITY_MAX_FRICTION_BOUND dFloat(1.0e15f)
|
||||
#define D_COMPLEMENTARITY_MIN_FRICTION_BOUND (-D_COMPLEMENTARITY_MAX_FRICTION_BOUND)
|
||||
|
||||
|
||||
class dSymmetricBiconjugateGradientSolve
|
||||
{
|
||||
public:
|
||||
dSymmetricBiconjugateGradientSolve () {}
|
||||
virtual ~dSymmetricBiconjugateGradientSolve () {}
|
||||
|
||||
virtual dFloat64 Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const;
|
||||
|
||||
protected:
|
||||
virtual void MatrixTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
|
||||
virtual bool InversePrecoditionerTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
|
||||
|
||||
private:
|
||||
dFloat64 DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const;
|
||||
void ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const;
|
||||
void Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const;
|
||||
};
|
||||
|
||||
class dComplementaritySolver
|
||||
{
|
||||
public:
|
||||
class dBodyState;
|
||||
class dBilateralJoint;
|
||||
|
||||
class dContact
|
||||
{
|
||||
public:
|
||||
dContact()
|
||||
:m_point(0.0f)
|
||||
,m_normal(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
dVector m_point;
|
||||
dVector m_normal;
|
||||
};
|
||||
|
||||
class dJacobian
|
||||
{
|
||||
public:
|
||||
dJacobian ()
|
||||
:m_linear(0.0f)
|
||||
,m_angular(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
dJacobian (const dVector& linear, const dVector& angular)
|
||||
:m_linear(linear)
|
||||
,m_angular(angular)
|
||||
{
|
||||
}
|
||||
|
||||
dVector m_linear;
|
||||
dVector m_angular;
|
||||
};
|
||||
|
||||
class dJacobianPair
|
||||
{
|
||||
public:
|
||||
dJacobian m_jacobian_J01;
|
||||
dJacobian m_jacobian_J10;
|
||||
};
|
||||
|
||||
class dJacobianColum
|
||||
{
|
||||
public:
|
||||
dFloat m_force;
|
||||
dFloat m_diagDamp;
|
||||
dFloat m_deltaAccel;
|
||||
dFloat m_invDJMinvJt;
|
||||
dFloat m_coordenateAccel;
|
||||
dFloat m_jointLowFriction;
|
||||
dFloat m_jointHighFriction;
|
||||
dBilateralJoint* m_frictionCallback;
|
||||
int m_normalIndex;
|
||||
};
|
||||
|
||||
class dParamInfo
|
||||
{
|
||||
public:
|
||||
dJacobianPair m_jacobians[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_jointAccel[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_jointLowFrictionCoef[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_jointHighFrictionCoef[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_diagonalRegularizer[D_MAX_PRAM_INFO_SIZE];
|
||||
int m_normalIndex[D_MAX_PRAM_INFO_SIZE];
|
||||
dBilateralJoint* m_frictionCallback[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_timestep;
|
||||
dFloat m_timestepInv;
|
||||
int m_count;
|
||||
};
|
||||
|
||||
class dJointAccelerationDecriptor
|
||||
{
|
||||
public:
|
||||
int m_rowsCount;
|
||||
dFloat m_timeStep;
|
||||
dFloat m_invTimeStep;
|
||||
dFloat m_firstPassCoefFlag;
|
||||
dJacobianPair* m_rowMatrix;
|
||||
dJacobianColum* m_colMatrix;
|
||||
};
|
||||
|
||||
class dBilateralJoint
|
||||
{
|
||||
public:
|
||||
dBilateralJoint()
|
||||
:m_state0(NULL)
|
||||
,m_state1(NULL)
|
||||
,m_ordinals(0x050403020100ll)
|
||||
,m_start(0)
|
||||
,m_count(0)
|
||||
,m_dof(0)
|
||||
{
|
||||
memset(m_rowIsMotor, 0, sizeof (m_rowIsMotor));
|
||||
memset(m_motorAcceleration, 0, sizeof (m_motorAcceleration));
|
||||
memset(m_jointFeebackForce, 0, sizeof (m_jointFeebackForce));
|
||||
}
|
||||
|
||||
virtual ~dBilateralJoint(){}
|
||||
|
||||
virtual void Init (dBodyState* const state0, dBodyState* const state1);
|
||||
virtual void JacobianDerivative (dParamInfo* const constraintParams) = 0;
|
||||
virtual void UpdateSolverForces (const dJacobianPair* const jacobians) const = 0;
|
||||
virtual void JointAccelerations (dJointAccelerationDecriptor* const accelParam);
|
||||
virtual void SpecialSolverFrictionCallback(const dFloat* const force, dFloat* const lowFriction, dFloat* const highFriction) const {}
|
||||
|
||||
void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle);
|
||||
void AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir);
|
||||
void AddContactRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir, dFloat restitution);
|
||||
|
||||
dFloat GetRowAccelaration(dParamInfo* const constraintParams) const;
|
||||
void SetRowAccelaration(dParamInfo* const constraintParams, dFloat accel);
|
||||
|
||||
dFloat CalculateMassMatrixDiagonal(dParamInfo* const constraintParams) const;
|
||||
dFloat CalculateAngle (const dVector& planeDir, const dVector& cosDir, const dVector& sinDir) const;
|
||||
|
||||
dFloat m_motorAcceleration[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_jointFeebackForce[D_MAX_PRAM_INFO_SIZE];
|
||||
int m_rowIsMotor[D_MAX_PRAM_INFO_SIZE];
|
||||
dBodyState* m_state0;
|
||||
dBodyState* m_state1;
|
||||
union
|
||||
{
|
||||
long long m_ordinals;
|
||||
char m_sourceJacobianIndex[8];
|
||||
};
|
||||
int m_start;
|
||||
int m_count;
|
||||
int m_dof;
|
||||
|
||||
friend class dBodyState;
|
||||
friend class dComplementaritySolver;
|
||||
};
|
||||
|
||||
class dFrictionLessContactJoint: public dBilateralJoint
|
||||
{
|
||||
public:
|
||||
dFrictionLessContactJoint()
|
||||
:dBilateralJoint()
|
||||
,m_restitution(0.0f)
|
||||
,m_count (0)
|
||||
{}
|
||||
virtual ~dFrictionLessContactJoint(){}
|
||||
|
||||
void SetContacts (int count, dContact* const contacts, dFloat restitution);
|
||||
|
||||
protected:
|
||||
void UpdateSolverForces (const dJacobianPair* const jacobians) const {}
|
||||
|
||||
static inline int CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy);
|
||||
int ReduceContacts (int count, dContact* const contacts, dFloat tol);
|
||||
void JacobianDerivative (dParamInfo* const constraintParams);
|
||||
void JointAccelerations (dJointAccelerationDecriptor* const params);
|
||||
|
||||
dContact m_contacts[D_MAX_PRAM_INFO_SIZE];
|
||||
dFloat m_restitution;
|
||||
int m_count;
|
||||
};
|
||||
|
||||
class dBodyState
|
||||
{
|
||||
public:
|
||||
dBodyState();
|
||||
virtual ~dBodyState() {}
|
||||
|
||||
dFloat GetMass () const;
|
||||
void SetMass (dFloat mass);
|
||||
|
||||
dFloat GetInvMass () const;
|
||||
|
||||
void SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz);
|
||||
void GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const;
|
||||
|
||||
const dMatrix& GetInertia() const;
|
||||
const dMatrix& GetInvInertia() const;
|
||||
|
||||
void SetVeloc (const dVector& veloc);
|
||||
void SetOmega (const dVector& omega);
|
||||
const dVector& GetOmega() const;
|
||||
const dVector& GetVelocity() const;
|
||||
dVector CalculatePointVelocity (const dVector& point) const;
|
||||
|
||||
void UpdateInertia();
|
||||
|
||||
void SetMatrix (const dMatrix& matrix);
|
||||
const dMatrix& GetMatrix () const;
|
||||
|
||||
void SetLocalMatrix (const dMatrix& matrix);
|
||||
const dMatrix& GetLocalMatrix () const;
|
||||
|
||||
void SetForce (const dVector& force);
|
||||
void SetTorque (const dVector& torque);
|
||||
const dVector& GetForce () const;
|
||||
const dVector& GetTorque () const;
|
||||
|
||||
const dVector& GetCOM () const;
|
||||
|
||||
void IntegrateVelocity (dFloat timestep);
|
||||
void IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque);
|
||||
|
||||
protected:
|
||||
virtual void ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega);
|
||||
|
||||
dMatrix m_matrix;
|
||||
dMatrix m_localFrame;
|
||||
dMatrix m_inertia;
|
||||
dMatrix m_invInertia;
|
||||
|
||||
dVector m_localInertia;
|
||||
dVector m_localInvInertia;
|
||||
|
||||
dVector m_veloc;
|
||||
dVector m_omega;
|
||||
dVector m_externalForce;
|
||||
dVector m_externalTorque;
|
||||
dVector m_globalCentreOfMass;
|
||||
|
||||
dFloat m_mass;
|
||||
dFloat m_invMass;
|
||||
int m_myIndex;
|
||||
|
||||
friend class dBilateralJoint;
|
||||
friend class dComplementaritySolver;
|
||||
};
|
||||
|
||||
public:
|
||||
dComplementaritySolver() {};
|
||||
virtual ~dComplementaritySolver() {};
|
||||
|
||||
virtual int GetActiveJoints (dBilateralJoint** const jointArray, int bufferSize)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual int BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount);
|
||||
virtual void CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user