martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Formatted rbdlsim.cc

master
Martin Felis 2020-10-16 11:06:24 +02:00
parent 823072ad72
commit 31cd1c3f0b
1 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/rbdlsim.cc
View File

@ -299,7 +299,6 @@ void CalcImpulseVariables(
const VectorNd& q = body->q; const VectorNd& q = body->q;
const VectorNd& qdot = body->qdot; const VectorNd& qdot = body->qdot;
// Calculate local coordinates of the contact point // Calculate local coordinates of the contact point
UpdateKinematicsCustom(*model, &q, nullptr, nullptr); UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
Vector3d point_local_b = Vector3d point_local_b =
@ -311,21 +310,32 @@ void CalcImpulseVariables(
*MInv = M.inverse(); *MInv = M.inverse();
MatrixNd G_constr(MatrixNd::Zero(3, ndof)); MatrixNd G_constr(MatrixNd::Zero(3, ndof));
CalcPointJacobian( CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false);
*model,
q,
body_index,
point_local_b,
G_constr,
false);
(*jac) = dir.transpose() * G_constr; (*jac) = dir.transpose() * G_constr;
*G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose(); *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
} }
void PrepareConstraintImpulse (SimBody* body_a, SimBody* body_b, CollisionInfo& cinfo) { void PrepareConstraintImpulse(
CalcImpulseVariables (body_a, cinfo.mBodyAIndex, cinfo.posA, cinfo.dir, &cinfo.MInvA, &cinfo.jacA, &cinfo.GMInvGTA); SimBody* body_a,
CalcImpulseVariables (body_b, cinfo.mBodyBIndex, cinfo.posB, -cinfo.dir, &cinfo.MInvB, &cinfo.jacB, &cinfo.GMInvGTB); SimBody* body_b,
CollisionInfo& cinfo) {
CalcImpulseVariables(
body_a,
cinfo.mBodyAIndex,
cinfo.posA,
cinfo.dir,
&cinfo.MInvA,
&cinfo.jacA,
&cinfo.GMInvGTA);
CalcImpulseVariables(
body_b,
cinfo.mBodyBIndex,
cinfo.posB,
-cinfo.dir,
&cinfo.MInvB,
&cinfo.jacB,
&cinfo.GMInvGTB);
} }
void CalcConstraintImpulse( void CalcConstraintImpulse(
@ -358,21 +368,20 @@ void ApplyConstraintImpulse(
SimBody* body, SimBody* body,
const MatrixNd& MInv, const MatrixNd& MInv,
const VectorNd& jac, const VectorNd& jac,
double impulse double impulse) {}
) {
}
void ApplyConstraintImpulse( void ApplyConstraintImpulse(
SimBody* body_a, SimBody* body_a,
SimBody* body_b, SimBody* body_b,
CollisionInfo& cinfo) { CollisionInfo& cinfo) {
if (!body_a->mIsStatic) { if (!body_a->mIsStatic) {
body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.dir.transpose() * cinfo.accumImpulseB); body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
* (-cinfo.dir.transpose() * cinfo.accumImpulseB);
} }
if (!body_b->mIsStatic) { if (!body_b->mIsStatic) {
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (-cinfo.dir.transpose() * cinfo.accumImpulseB); body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
* (-cinfo.dir.transpose() * cinfo.accumImpulseB);
} }
} }