295 lines
7.9 KiB
C++
295 lines
7.9 KiB
C++
/* 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
|
|
|