00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "btSolve2LinearConstraint.h"
00019
00020 #include "BulletDynamics/Dynamics/btRigidBody.h"
00021 #include "LinearMath/btVector3.h"
00022 #include "btJacobianEntry.h"
00023
00024
00025 void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
00026 btRigidBody* body1,
00027 btRigidBody* body2,
00028
00029 const btMatrix3x3& world2A,
00030 const btMatrix3x3& world2B,
00031
00032 const btVector3& invInertiaADiag,
00033 const btScalar invMassA,
00034 const btVector3& linvelA,const btVector3& angvelA,
00035 const btVector3& rel_posA1,
00036 const btVector3& invInertiaBDiag,
00037 const btScalar invMassB,
00038 const btVector3& linvelB,const btVector3& angvelB,
00039 const btVector3& rel_posA2,
00040
00041 btScalar depthA, const btVector3& normalA,
00042 const btVector3& rel_posB1,const btVector3& rel_posB2,
00043 btScalar depthB, const btVector3& normalB,
00044 btScalar& imp0,btScalar& imp1)
00045 {
00046 (void)linvelA;
00047 (void)linvelB;
00048 (void)angvelB;
00049 (void)angvelA;
00050
00051
00052
00053 imp0 = btScalar(0.);
00054 imp1 = btScalar(0.);
00055
00056 btScalar len = btFabs(normalA.length()) - btScalar(1.);
00057 if (btFabs(len) >= SIMD_EPSILON)
00058 return;
00059
00060 btAssert(len < SIMD_EPSILON);
00061
00062
00063
00064 btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
00065 invInertiaBDiag,invMassB);
00066 btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
00067 invInertiaBDiag,invMassB);
00068
00069
00070
00071
00072 const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
00073 const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
00074
00075
00076 btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
00077
00078
00079
00080 const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
00081 const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095 btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
00096 btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
00097
00098
00099
00100
00101 imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
00102 imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
00103
00104
00105
00106
00107
00108
00109
00110 }
00111
00112
00113
00114 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
00115 btRigidBody* body1,
00116 btRigidBody* body2,
00117 const btMatrix3x3& world2A,
00118 const btMatrix3x3& world2B,
00119
00120 const btVector3& invInertiaADiag,
00121 const btScalar invMassA,
00122 const btVector3& linvelA,const btVector3& angvelA,
00123 const btVector3& rel_posA1,
00124 const btVector3& invInertiaBDiag,
00125 const btScalar invMassB,
00126 const btVector3& linvelB,const btVector3& angvelB,
00127 const btVector3& rel_posA2,
00128
00129 btScalar depthA, const btVector3& normalA,
00130 const btVector3& rel_posB1,const btVector3& rel_posB2,
00131 btScalar depthB, const btVector3& normalB,
00132 btScalar& imp0,btScalar& imp1)
00133 {
00134
00135 (void)linvelA;
00136 (void)linvelB;
00137 (void)angvelA;
00138 (void)angvelB;
00139
00140
00141
00142 imp0 = btScalar(0.);
00143 imp1 = btScalar(0.);
00144
00145 btScalar len = btFabs(normalA.length()) - btScalar(1.);
00146 if (btFabs(len) >= SIMD_EPSILON)
00147 return;
00148
00149 btAssert(len < SIMD_EPSILON);
00150
00151
00152
00153 btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
00154 invInertiaBDiag,invMassB);
00155 btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
00156 invInertiaBDiag,invMassB);
00157
00158
00159
00160
00161 const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
00162 const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
00163
00164
00165 const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
00166 const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179 btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
00180 btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
00181
00182
00183
00184
00185 imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
00186 imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
00187
00188
00189
00190
00191
00192
00193
00194 if ( imp0 > btScalar(0.0))
00195 {
00196 if ( imp1 > btScalar(0.0) )
00197 {
00198
00199 }
00200 else
00201 {
00202 imp1 = btScalar(0.);
00203
00204
00205 imp0 = dv0 / jacA.getDiagonal();
00206 if ( imp0 > btScalar(0.0) )
00207 {
00208 } else
00209 {
00210 imp0 = btScalar(0.);
00211 }
00212 }
00213 }
00214 else
00215 {
00216 imp0 = btScalar(0.);
00217
00218 imp1 = dv1 / jacB.getDiagonal();
00219 if ( imp1 <= btScalar(0.0) )
00220 {
00221 imp1 = btScalar(0.);
00222
00223 imp0 = dv0 / jacA.getDiagonal();
00224 if ( imp0 > btScalar(0.0) )
00225 {
00226 } else
00227 {
00228 imp0 = btScalar(0.);
00229 }
00230 } else
00231 {
00232 }
00233 }
00234 }
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255