My Project: dynamics.mod/NewtonDynamics/packages/dMath/dLinearAlgebra.h Source File

My Project

dLinearAlgebra.h
Go to the documentation of this file.
1 /* Copyright (c) <2009> <Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely
10 */
11
12 #include "dStdAfxMath.h"
13 #include "dMathDefines.h"
14 #include "dVector.h"
15 #include "dMatrix.h"
16 #include "dQuaternion.h"
17
18
19 #ifndef __D_LINEAR_ALGEBRA_H__
20 #define __D_LINEAR_ALGEBRA_H__
21
22 #ifdef _MSC_VER
23 #pragma warning (disable: 4100) //unreferenced formal parameter
24 #endif
25
26
27 #define D_MAX_PRAM_INFO_SIZE 16
28 #define D_MAX_PLACEMENT_CONTACTS 128
29
30 class dSymmetricBiconjugateGradientSolve
31 {
32 public:
33 dSymmetricBiconjugateGradientSolve () {}
34 virtual ~dSymmetricBiconjugateGradientSolve () {}
35
36 virtual dFloat64 Solve (int size, dFloat64 tolerance, dFloat64* const x, const dFloat64* const b) const;
37
38 protected:
39 virtual void MatrixTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
40 virtual bool InversePrecoditionerTimeVector (dFloat64* const out, const dFloat64* const v) const = 0;
41
42 private:
43 dFloat64 DotProduct (int size, const dFloat64* const b, const dFloat64* const c) const;
44 void ScaleAdd (int size, dFloat64* const a, const dFloat64* const b, dFloat64 scale, const dFloat64* const c) const;
45 void Sub (int size, dFloat64* const a, const dFloat64* const b, const dFloat64* const c) const;
46 };
47
48 class dComplemtaritySolver
49 {
50 public:
51 class dBodyState;
52 class dBilateralJoint;
53
54
55 class dContact
56 {
57 public:
58 dVector m_point;
59 dVector m_normal;
60 };
61
62 class dJacobian
63 {
64 public:
65 dVector m_linear;
66 dVector m_angular;
67 };
68
69 class dJacobianPair
70 {
71 public:
72 dJacobian m_jacobian_IM0;
73 dJacobian m_jacobian_IM1;
74 };
75
76 class dJacobianColum
77 {
78 public:
79 dFloat m_force;
80 dFloat m_diagDamp;
81 dFloat m_deltaAccel;
82 dFloat m_invDJMinvJt;
83 dFloat m_coordenateAccel;
84 dFloat m_jointLowFriction;
85 dFloat m_jointHighFriction;
86 };
87
88 class dParamInfo
89 {
90 public:
91 dJacobianPair m_jacobians[D_MAX_PRAM_INFO_SIZE];
92 dFloat m_jointAccel[D_MAX_PRAM_INFO_SIZE];
93 dFloat m_jointLowFriction[D_MAX_PRAM_INFO_SIZE];
94 dFloat m_jointHighFriction[D_MAX_PRAM_INFO_SIZE];
95 dFloat m_timestep;
96 dFloat m_timestepInv;
97 int m_count;
98 };
99
100 class dPointDerivativeParam
101 {
102 public:
103 dVector m_r0;
104 dVector m_posit0;
105 dVector m_veloc0;
106 dVector m_centripetal0;
107
108 dVector m_r1;
109 dVector m_posit1;
110 dVector m_veloc1;
111 dVector m_centripetal1;
112 };
113
114 class dJointAccelerationDecriptor
115 {
116 public:
117 int m_rowsCount;
118 dFloat m_timeStep;
119 dFloat m_invTimeStep;
120 dFloat m_firstPassCoefFlag;
121 dJacobianPair* m_rowMatrix;
122 dJacobianColum* m_colMatrix;
123 };
124
125 class dBilateralJoint
126 {
127 public:
128 dBilateralJoint()
129 :m_state0(NULL)
130 ,m_state1(NULL)
131 ,m_start(0)
132 ,m_count(0)
133 {
134 }
135 virtual ~dBilateralJoint(){}
136
137 virtual void Init (dBodyState* const state0, dBodyState* const state1);
138
139 protected:
140 virtual void JacobianDerivative (dParamInfo* const constraintParams) = 0;
141 virtual void UpdateSolverForces (const dJacobianPair* const jacobians) const = 0;
142 virtual void JointAccelerations (dJointAccelerationDecriptor* const accelParam);
143
144 void InitPointParam (dPointDerivativeParam& param, const dVector& pivot) const;
145 void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir, dFloat jointAngle);
146 void AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir);
147 void AddAngularRowJacobian (dParamInfo* const constraintParams, const dVector& dir0, const dVector& dir1, dFloat ratio);
148 void CalculatePointDerivative (dParamInfo* const constraintParams, const dVector& dir, const dPointDerivativeParam& param);
149
150 dFloat m_motorAcceleration[D_MAX_PRAM_INFO_SIZE];
151 dFloat m_jointFeebackForce[D_MAX_PRAM_INFO_SIZE];
152 int m_rowIsMotor[D_MAX_PRAM_INFO_SIZE];
153 dBodyState* m_state0;
154 dBodyState* m_state1;
155 int m_start;
156 int m_count;
157
158 friend class dBodyState;
159 friend class dComplemtaritySolver;
160 };
161
162 class dFrictionLessContactJoint: public dBilateralJoint
163 {
164 public:
165 dFrictionLessContactJoint()
166 :dBilateralJoint()
167 ,m_restitution(0.0f)
168 ,m_count (0)
169 {}
170 virtual ~dFrictionLessContactJoint(){}
171
172 void SetContacts (int count, dContact* const contacts, dFloat restitution);
173
174 protected:
175 void UpdateSolverForces (const dJacobianPair* const jacobians) const {}
176
177 static inline int CompareContact (const dContact* const contactA, const dContact* const contactB, void* dommy);
178 int ReduceContacts (int count, dContact* const contacts, dFloat tol);
179 void JacobianDerivative (dParamInfo* const constraintParams);
180 void JointAccelerations (dJointAccelerationDecriptor* const params);
181
182 dContact m_contacts[D_MAX_PRAM_INFO_SIZE];
183 dFloat m_restitution;
184 int m_count;
185 };
186
187
188 class dBodyState
189 {
190 public:
191 dBodyState();
192 virtual ~dBodyState() {}
193
194 dFloat GetMass () const;
195 void SetMass (dFloat mass);
196
197 dFloat GetInvMass () const;
198
199 void SetInertia (dFloat Ixx, dFloat Iyy, dFloat Izz);
200 void GetInertia (dFloat& Ixx, dFloat& Iyy, dFloat& Izz) const;
201
202 void SetVeloc (const dVector& veloc);
203 void SetOmega (const dVector& omega);
204 const dVector& GetOmega() const;
205 const dVector& GetVelocity() const;
206
207 void UpdateInertia();
208
209 void SetMatrix (const dMatrix& matrix);
210 const dMatrix& GetMatrix () const;
211
212 void SetLocalMatrix (const dMatrix& matrix);
213 const dMatrix& GetLocalMatrix () const;
214
215 void SetForce (const dVector& force);
216 void SetTorque (const dVector& torque);
217 const dVector& GetForce () const;
218 const dVector& GetTorque () const;
219
220 const dVector& GetCenterOfMass () const;
221
222 void IntegrateVelocity (dFloat timestep);
223
224 protected:
225 virtual void IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque);
226 virtual void ApplyNetForceAndTorque (dFloat invTimestep, const dVector& veloc, const dVector& omega);
227
228 dMatrix m_matrix;
229 dMatrix m_localFrame;
230 dMatrix m_inertia;
231 dMatrix m_invInertia;
232
233 dVector m_localInertia;
234 dVector m_localInvInertia;
235
236 dVector m_veloc;
237 dVector m_omega;
238 dVector m_externalForce;
239 dVector m_externalTorque;
240 dVector m_globalCentreOfMass;
241
242 dFloat m_mass;
243 dFloat m_invMass;
244 int m_myIndex;
245
246 friend class dBilateralJoint;
247 friend class dComplemtaritySolver;
248 };
249
250
251
252 public:
253 dComplemtaritySolver() {};
254 virtual ~dComplemtaritySolver() {};
255
256 virtual int GetActiveJoints (dBilateralJoint** const jointArray, int bufferSize)
257 {
258 return 0;
259 }
260
261 virtual int BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount);
262 virtual void CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray);
263 };
264
265
266 #endif
267
dComplemtaritySolver::dContact::m_normal
dVector m_normal
Definition: dLinearAlgebra.h:59
dComplemtaritySolver::dJointAccelerationDecriptor::m_rowMatrix
dJacobianPair * m_rowMatrix
Definition: dLinearAlgebra.h:121
dComplemtaritySolver::dJacobianColum::m_jointHighFriction
dFloat m_jointHighFriction
Definition: dLinearAlgebra.h:85
dComplemtaritySolver::~dComplemtaritySolver
virtual ~dComplemtaritySolver()
Definition: dLinearAlgebra.h:254
dComplemtaritySolver::dBodyState::m_invMass
dFloat m_invMass
Definition: dLinearAlgebra.h:243
dComplemtaritySolver::dBodyState::m_globalCentreOfMass
dVector m_globalCentreOfMass
Definition: dLinearAlgebra.h:240
dComplemtaritySolver::dContact::m_point
dVector m_point
Definition: dLinearAlgebra.h:58
dComplemtaritySolver::dPointDerivativeParam::m_centripetal0
dVector m_centripetal0
Definition: dLinearAlgebra.h:106
dComplemtaritySolver::dJointAccelerationDecriptor::m_invTimeStep
dFloat m_invTimeStep
Definition: dLinearAlgebra.h:119
dComplemtaritySolver::dContact
Definition: dLinearAlgebra.h:55
dComplemtaritySolver::dBodyState::m_veloc
dVector m_veloc
Definition: dLinearAlgebra.h:236
dComplemtaritySolver::dPointDerivativeParam::m_veloc0
dVector m_veloc0
Definition: dLinearAlgebra.h:105
dComplemtaritySolver::dPointDerivativeParam::m_posit1
dVector m_posit1
Definition: dLinearAlgebra.h:109
dComplemtaritySolver::CalculateReactionsForces
virtual void CalculateReactionsForces(int bodyCount, dBodyState **const bodyArray, int jointCount, dBilateralJoint **const jointArray, dFloat timestep, dJacobianPair *const jacobianArray, dJacobianColum *const jacobianColumnArray)
Definition: dLinearAlgebra.cpp:707
dComplemtaritySolver::dBodyState::m_mass
dFloat m_mass
Definition: dLinearAlgebra.h:242
dMatrix
Definition: dMatrix.h:28
dComplemtaritySolver::dBodyState::m_externalForce
dVector m_externalForce
Definition: dLinearAlgebra.h:238
dComplemtaritySolver::dFrictionLessContactJoint::m_count
int m_count
Definition: dLinearAlgebra.h:184
dComplemtaritySolver::dJacobian::m_linear
dVector m_linear
Definition: dLinearAlgebra.h:65
dComplemtaritySolver::dBodyState::m_myIndex
int m_myIndex
Definition: dLinearAlgebra.h:244
dSymmetricBiconjugateGradientSolve::Solve
virtual dFloat64 Solve(int size, dFloat64 tolerance, dFloat64 *const x, const dFloat64 *const b) const
Definition: dLinearAlgebra.cpp:50
dComplemtaritySolver::dFrictionLessContactJoint
Definition: dLinearAlgebra.h:162
dComplemtaritySolver::dJointAccelerationDecriptor::m_colMatrix
dJacobianColum * m_colMatrix
Definition: dLinearAlgebra.h:122
dComplemtaritySolver::dJacobian::m_angular
dVector m_angular
Definition: dLinearAlgebra.h:66
dFloat
float dFloat
Definition: dMathDefines.h:27
dComplemtaritySolver::dPointDerivativeParam::m_centripetal1
dVector m_centripetal1
Definition: dLinearAlgebra.h:111
dComplemtaritySolver::dBodyState::m_omega
dVector m_omega
Definition: dLinearAlgebra.h:237
D_MAX_PRAM_INFO_SIZE
#define D_MAX_PRAM_INFO_SIZE
Definition: dLinearAlgebra.h:27
dComplemtaritySolver::dBodyState::~dBodyState
virtual ~dBodyState()
Definition: dLinearAlgebra.h:192
dComplemtaritySolver::dBodyState::m_localInertia
dVector m_localInertia
Definition: dLinearAlgebra.h:233
dComplemtaritySolver::GetActiveJoints
virtual int GetActiveJoints(dBilateralJoint **const jointArray, int bufferSize)
Definition: dLinearAlgebra.h:256
dMathDefines.h
dComplemtaritySolver::dBodyState::m_localFrame
dMatrix m_localFrame
Definition: dLinearAlgebra.h:229
dComplemtaritySolver::dParamInfo::m_count
int m_count
Definition: dLinearAlgebra.h:97
dComplemtaritySolver::dPointDerivativeParam::m_posit0
dVector m_posit0
Definition: dLinearAlgebra.h:104
dComplemtaritySolver::dParamInfo
Definition: dLinearAlgebra.h:88
dComplemtaritySolver::dBodyState::m_matrix
dMatrix m_matrix
Definition: dLinearAlgebra.h:228
dQuaternion.h
dSymmetricBiconjugateGradientSolve::dSymmetricBiconjugateGradientSolve
dSymmetricBiconjugateGradientSolve()
Definition: dLinearAlgebra.h:33
dComplemtaritySolver::dJacobianColum::m_coordenateAccel
dFloat m_coordenateAccel
Definition: dLinearAlgebra.h:83
dComplemtaritySolver::dJacobianPair::m_jacobian_IM0
dJacobian m_jacobian_IM0
Definition: dLinearAlgebra.h:72
dComplemtaritySolver::BuildJacobianMatrix
virtual int BuildJacobianMatrix(int jointCount, dBilateralJoint **const jointArray, dFloat timestep, dJacobianPair *const jacobianArray, dJacobianColum *const jacobianColumnArray, int maxRowCount)
Definition: dLinearAlgebra.cpp:638
dgJointCallbackParam
Definition: dgConstraint.h:63
dComplemtaritySolver::dJacobian
Definition: dLinearAlgebra.h:62
dComplemtaritySolver::dBilateralJoint
Definition: dLinearAlgebra.h:125
dMatrix.h
dComplemtaritySolver::dComplemtaritySolver
dComplemtaritySolver()
Definition: dLinearAlgebra.h:253
dComplemtaritySolver::dBilateralJoint::m_state1
dBodyState * m_state1
Definition: dLinearAlgebra.h:154
dComplemtaritySolver
Definition: dLinearAlgebra.h:48
dComplemtaritySolver::dJointAccelerationDecriptor
Definition: dLinearAlgebra.h:114
dComplemtaritySolver::dBodyState::m_inertia
dMatrix m_inertia
Definition: dLinearAlgebra.h:230
dComplemtaritySolver::dBilateralJoint::m_start
int m_start
Definition: dLinearAlgebra.h:155
dComplemtaritySolver::dJacobianColum::m_jointLowFriction
dFloat m_jointLowFriction
Definition: dLinearAlgebra.h:84
dFloat64
double dFloat64
Definition: dMathDefines.h:32
dComplemtaritySolver::dBodyState::m_invInertia
dMatrix m_invInertia
Definition: dLinearAlgebra.h:231
dComplemtaritySolver::dPointDerivativeParam::m_r1
dVector m_r1
Definition: dLinearAlgebra.h:108
dComplemtaritySolver::dJacobianPair
Definition: dLinearAlgebra.h:69
dComplemtaritySolver::dJacobianColum
Definition: dLinearAlgebra.h:76
dSymmetricBiconjugateGradientSolve::MatrixTimeVector
virtual void MatrixTimeVector(dFloat64 *const out, const dFloat64 *const v) const =0
dComplemtaritySolver::dFrictionLessContactJoint::m_restitution
dFloat m_restitution
Definition: dLinearAlgebra.h:183
dComplemtaritySolver::dBodyState::m_externalTorque
dVector m_externalTorque
Definition: dLinearAlgebra.h:239
dComplemtaritySolver::dPointDerivativeParam::m_veloc1
dVector m_veloc1
Definition: dLinearAlgebra.h:110
dComplemtaritySolver::dPointDerivativeParam
Definition: dLinearAlgebra.h:100
dComplemtaritySolver::dFrictionLessContactJoint::dFrictionLessContactJoint
dFrictionLessContactJoint()
Definition: dLinearAlgebra.h:165
dComplemtaritySolver::dJointAccelerationDecriptor::m_rowsCount
int m_rowsCount
Definition: dLinearAlgebra.h:117
timestep
dgFloat32 timestep
Definition: dgBallConstraint.h:34
dComplemtaritySolver::dBilateralJoint::~dBilateralJoint
virtual ~dBilateralJoint()
Definition: dLinearAlgebra.h:135
dComplemtaritySolver::dBilateralJoint::m_count
int m_count
Definition: dLinearAlgebra.h:156
dComplemtaritySolver::dJacobianColum::m_diagDamp
dFloat m_diagDamp
Definition: dLinearAlgebra.h:80
dComplemtaritySolver::dBilateralJoint::dBilateralJoint
dBilateralJoint()
Definition: dLinearAlgebra.h:128
dComplemtaritySolver::dPointDerivativeParam::m_r0
dVector m_r0
Definition: dLinearAlgebra.h:103
dComplemtaritySolver::dJointAccelerationDecriptor::m_timeStep
dFloat m_timeStep
Definition: dLinearAlgebra.h:118
dComplemtaritySolver::dBilateralJoint::m_state0
dBodyState * m_state0
Definition: dLinearAlgebra.h:153
dComplemtaritySolver::dJointAccelerationDecriptor::m_firstPassCoefFlag
dFloat m_firstPassCoefFlag
Definition: dLinearAlgebra.h:120
dComplemtaritySolver::dBodyState::m_localInvInertia
dVector m_localInvInertia
Definition: dLinearAlgebra.h:234
dComplemtaritySolver::dJacobianPair::m_jacobian_IM1
dJacobian m_jacobian_IM1
Definition: dLinearAlgebra.h:73
dComplemtaritySolver::dJacobianColum::m_deltaAccel
dFloat m_deltaAccel
Definition: dLinearAlgebra.h:81
dComplemtaritySolver::dParamInfo::m_timestepInv
dFloat m_timestepInv
Definition: dLinearAlgebra.h:96
dVector.h
dSymmetricBiconjugateGradientSolve::InversePrecoditionerTimeVector
virtual bool InversePrecoditionerTimeVector(dFloat64 *const out, const dFloat64 *const v) const =0
dSymmetricBiconjugateGradientSolve::~dSymmetricBiconjugateGradientSolve
virtual ~dSymmetricBiconjugateGradientSolve()
Definition: dLinearAlgebra.h:34
dSymmetricBiconjugateGradientSolve
Definition: dLinearAlgebra.h:30
dComplemtaritySolver::dFrictionLessContactJoint::~dFrictionLessContactJoint
virtual ~dFrictionLessContactJoint()
Definition: dLinearAlgebra.h:170
dStdAfxMath.h
dVector
Definition: dVector.h:54
dComplemtaritySolver::dJacobianColum::m_invDJMinvJt
dFloat m_invDJMinvJt
Definition: dLinearAlgebra.h:82
dComplemtaritySolver::dFrictionLessContactJoint::UpdateSolverForces
void UpdateSolverForces(const dJacobianPair *const jacobians) const
Definition: dLinearAlgebra.h:175
dComplemtaritySolver::dJacobianColum::m_force
dFloat m_force
Definition: dLinearAlgebra.h:79
dComplemtaritySolver::dBodyState
Definition: dLinearAlgebra.h:188
dComplemtaritySolver::dParamInfo::m_timestep
dFloat m_timestep
Definition: dLinearAlgebra.h:95