dReal ServoMotor::getTorque() {
// code from Jeff Shim
osg::Vec3 torque1(fback_.t1[0], fback_.t1[1], fback_.t1[2] );
osg::Vec3 torque2(fback_.t2[0], fback_.t2[1], fback_.t2[2] );
osg::Vec3 force1(fback_.f1[0], fback_.f1[1], fback_.f1[2] );
osg::Vec3 force2(fback_.f2[0], fback_.f2[1], fback_.f2[2] );
const double* p1 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),0) );
const double* p2 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),1) );
osg::Vec3 pos1(p1[0], p1[1], p1[2]);
osg::Vec3 pos2(p2[0], p2[1], p2[2]);
dVector3 odeAnchor;
dJointGetHingeAnchor ( joint_->getJoint(), odeAnchor );
osg::Vec3 anchor(odeAnchor[0], odeAnchor[1], odeAnchor[2]);
osg::Vec3 ftorque1 = torque1 - (force1^(pos1-anchor));// torq by motor = total torq - constraint torq
osg::Vec3 ftorque2 = torque2 - (force2^(pos2-anchor));// opposite direction - use if this is necessary
dVector3 odeAxis;
dJointGetHingeAxis ( joint_->getJoint(), odeAxis);
osg::Vec3 axis(odeAxis[0], odeAxis[1], odeAxis[2] );
axis.normalize();
double torque = ftorque1 * axis;
//printf ("torque: % 1.10f\n", torque);
return torque;
}
void dgWorldDynamicUpdate::BuildJacobianMatrix (const dgBodyInfo* const bodyInfoArray, const dgJointInfo* const jointInfo, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 forceImpulseScale) const
{
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgBody* const body0 = bodyInfoArray[m0].m_body;
const dgBody* const body1 = bodyInfoArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgVector invMass0(body0->m_invMass[3]);
const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix;
const dgVector invMass1(body1->m_invMass[3]);
const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix;
dgVector force0(dgVector::m_zero);
dgVector torque0(dgVector::m_zero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = ((dgDynamicBody*)body0)->m_externalForce;
torque0 = ((dgDynamicBody*)body0)->m_externalTorque;
}
dgVector force1(dgVector::m_zero);
dgVector torque1(dgVector::m_zero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = ((dgDynamicBody*)body1)->m_externalForce;
torque1 = ((dgDynamicBody*)body1)->m_externalTorque;
}
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
dgJacobian forceAcc0;
dgJacobian forceAcc1;
forceAcc0.m_linear = dgVector::m_zero;
forceAcc0.m_angular = dgVector::m_zero;
forceAcc1.m_linear = dgVector::m_zero;
forceAcc1.m_angular = dgVector::m_zero;
for (dgInt32 i = 0; i < count; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f));
row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);
dgVector tmpAccel(row->m_JMinv.m_jacobianM0.m_linear * force0 + row->m_JMinv.m_jacobianM0.m_angular * torque0 +
row->m_JMinv.m_jacobianM1.m_linear * force1 + row->m_JMinv.m_jacobianM1.m_angular * torque1);
dgAssert(tmpAccel.m_w == dgFloat32(0.0f));
dgFloat32 extenalAcceleration = -(tmpAccel.AddHorizontal()).GetScalar();
row->m_deltaAccel = extenalAcceleration * forceImpulseScale;
row->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(row->m_jointFeebackForce);
const dgFloat32 force = row->m_jointFeebackForce->m_force * forceImpulseScale;
row->m_force = isBilateral ? dgClamp(force, row->m_lowerBoundFrictionCoefficent, row->m_upperBoundFrictionCoefficent) : force;
row->m_force0 = row->m_force;
row->m_maxImpact = dgFloat32(0.0f);
dgVector jMinvM0linear (scale0 * row->m_JMinv.m_jacobianM0.m_linear);
dgVector jMinvM0angular (scale0 * row->m_JMinv.m_jacobianM0.m_angular);
dgVector jMinvM1linear (scale1 * row->m_JMinv.m_jacobianM1.m_linear);
dgVector jMinvM1angular (scale1 * row->m_JMinv.m_jacobianM1.m_angular);
dgVector tmpDiag(jMinvM0linear * row->m_Jt.m_jacobianM0.m_linear + jMinvM0angular * row->m_Jt.m_jacobianM0.m_angular +
jMinvM1linear * row->m_Jt.m_jacobianM1.m_linear + jMinvM1angular * row->m_Jt.m_jacobianM1.m_angular);
dgAssert (tmpDiag.m_w == dgFloat32 (0.0f));
dgFloat32 diag = (tmpDiag.AddHorizontal()).GetScalar();
dgAssert(diag > dgFloat32(0.0f));
row->m_diagDamp = diag * row->m_stiffness;
diag *= (dgFloat32(1.0f) + row->m_stiffness);
row->m_jMinvJt = diag;
row->m_invJMinvJt = dgFloat32(1.0f) / diag;
dgAssert(dgCheckFloat(row->m_force));
dgVector val(row->m_force);
forceAcc0.m_linear += row->m_Jt.m_jacobianM0.m_linear * val;
forceAcc0.m_angular += row->m_Jt.m_jacobianM0.m_angular * val;
forceAcc1.m_linear += row->m_Jt.m_jacobianM1.m_linear * val;
forceAcc1.m_angular += row->m_Jt.m_jacobianM1.m_angular * val;
}
forceAcc0.m_linear = forceAcc0.m_linear * scale0;
forceAcc0.m_angular = forceAcc0.m_angular * scale0;
forceAcc1.m_linear = forceAcc1.m_linear * scale1;
forceAcc1.m_angular = forceAcc1.m_angular * scale1;
if (!body0->m_equilibrium) {
internalForces[m0].m_linear += forceAcc0.m_linear;
internalForces[m0].m_angular += forceAcc0.m_angular;
}
if (!body1->m_equilibrium) {
internalForces[m1].m_linear += forceAcc1.m_linear;
internalForces[m1].m_angular += forceAcc1.m_angular;
}
}
DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgJacobian* const internalForces)
{
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgDynamicBody* const body0 = (dgDynamicBody*)m_bodyArray[m0].m_body;
const dgDynamicBody* const body1 = (dgDynamicBody*)m_bodyArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgMatrix invInertia0 = body0->m_invWorldInertiaMatrix;
const dgMatrix invInertia1 = body1->m_invWorldInertiaMatrix;
const dgVector invMass0(body0->m_invMass[3]);
const dgVector invMass1(body1->m_invMass[3]);
dgVector force0(m_zero);
dgVector torque0(m_zero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = body0->m_externalForce;
torque0 = body0->m_externalTorque;
}
dgVector force1(m_zero);
dgVector torque1(m_zero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = body1->m_externalForce;
torque1 = body1->m_externalTorque;
}
jointInfo->m_preconditioner0 = dgFloat32(1.0f);
jointInfo->m_preconditioner1 = dgFloat32(1.0f);
if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f)) && !(body0->GetSkeleton() && body1->GetSkeleton())) {
const dgFloat32 mass0 = body0->GetMass().m_w;
const dgFloat32 mass1 = body1->GetMass().m_w;
if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) {
jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER);
} else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) {
jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER);
}
}
//dgSoaFloat forceAcc0(m_soaZero);
//dgSoaFloat forceAcc1(m_soaZero);
dgVector forceAcc0(m_zero);
dgVector torqueAcc0(m_zero);
dgVector forceAcc1(m_zero);
dgVector torqueAcc1(m_zero);
//const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
//const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
const dgVector weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
const dgVector weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
const dgFloat32 forceImpulseScale = dgFloat32(1.0f);
const dgFloat32 preconditioner0 = jointInfo->m_preconditioner0;
const dgFloat32 preconditioner1 = jointInfo->m_preconditioner1;
for (dgInt32 i = 0; i < count; i++) {
dgLeftHandSide* const row = &leftHandSide[index + i];
dgRightHandSide* const rhs = &rightHandSide[index + i];
row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);
//const dgSoaFloat& JMinvM0 = (dgSoaFloat&)row->m_JMinv.m_jacobianM0;
//const dgSoaFloat& JMinvM1 = (dgSoaFloat&)row->m_JMinv.m_jacobianM1;
//const dgSoaFloat tmpAccel((JMinvM0 * force0).MulAdd(JMinvM1, force1));
const dgJacobian& JMinvM0 = row->m_JMinv.m_jacobianM0;
const dgJacobian& JMinvM1 = row->m_JMinv.m_jacobianM1;
const dgVector tmpAccel(JMinvM0.m_linear * force0 + JMinvM0.m_angular * torque0 +
JMinvM1.m_linear * force1 + JMinvM1.m_angular * torque1);
dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal().GetScalar();
rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale;
rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(rhs->m_jointFeebackForce);
const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale;
rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force;
rhs->m_maxImpact = dgFloat32(0.0f);
//const dgSoaFloat& JtM0 = (dgSoaFloat&)row->m_Jt.m_jacobianM0;
//const dgSoaFloat& JtM1 = (dgSoaFloat&)row->m_Jt.m_jacobianM1;
//const dgSoaFloat tmpDiag((weight0 * JMinvM0 * JtM0).MulAdd(weight1, JMinvM1 * JtM1));
const dgJacobian& JtM0 = row->m_Jt.m_jacobianM0;
const dgJacobian& JtM1 = row->m_Jt.m_jacobianM1;
const dgVector tmpDiag(weight0 * (JMinvM0.m_linear * JtM0.m_linear + JMinvM0.m_angular * JtM0.m_angular) +
weight1 * (JMinvM1.m_linear * JtM1.m_linear + JMinvM1.m_angular * JtM1.m_angular));
dgFloat32 diag = tmpDiag.AddHorizontal().GetScalar();
dgAssert(diag > dgFloat32(0.0f));
rhs->m_diagDamp = diag * rhs->m_stiffness;
diag *= (dgFloat32(1.0f) + rhs->m_stiffness);
rhs->m_invJinvMJt = dgFloat32(1.0f) / diag;
dgVector f0(rhs->m_force * preconditioner0);
dgVector f1(rhs->m_force * preconditioner1);
//forceAcc0 = forceAcc0.MulAdd(JtM0, f0);
//forceAcc1 = forceAcc1.MulAdd(JtM1, f1);
forceAcc0 = forceAcc0 + JtM0.m_linear * f0;
torqueAcc0 = torqueAcc0 + JtM0.m_angular * f0;
forceAcc1 = forceAcc1 + JtM1.m_linear * f1;
torqueAcc1 = torqueAcc1 + JtM1.m_angular * f1;
}
if (m0) {
//dgSoaFloat& out = (dgSoaFloat&)internalForces[m0];
//dgScopeSpinPause lock(&m_bodyProxyArray[m0].m_lock);
//out = out + forceAcc0;
dgJacobian& out = internalForces[m0];
dgScopeSpinPause lock(&m_bodyProxyArray[m0].m_lock);
out.m_linear += forceAcc0;
out.m_angular += torqueAcc0;
}
if (m1) {
//dgSoaFloat& out = (dgSoaFloat&)internalForces[m1];
//dgScopeSpinPause lock(&m_bodyProxyArray[m1].m_lock);
//out = out + forceAcc1;
dgJacobian& out = internalForces[m1];
dgScopeSpinPause lock(&m_bodyProxyArray[m1].m_lock);
out.m_linear += forceAcc1;
out.m_angular += torqueAcc1;
}
}
