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;
}
/**
* @brief Moves particles forward based on 2nd order Runge-Kutta method
*
* @param[in] endTime Final time of the simulation
**/
void Runge_Kutta2::moveParticles(const double endTime) {
while (currentTime < endTime) {
const double dt = modifyTimeStep(1.0e-4f, init_dt); // implement dynamic timstep (if necessary):
const doubleV vdt = set1_pd(dt); // store timestep as vector const
const cloud_index numParticles = cloud->n;
operate1(currentTime);
force1(currentTime); // compute net force1
BEGIN_PARALLEL_FOR(i, e, numParticles, DOUBLE_STRIDE, static) // calculate k1 and l1 for entire cloud
const doubleV vmass = load_pd(cloud->mass + i); // load ith and (i+1)th mass into vector
// assign force pointers for stylistic purposes:
double * const pFx = cloud->forceX + i;
double * const pFy = cloud->forceY + i;
store_pd(cloud->k1 + i, div_pd(mul_pd(vdt, load_pd(pFx)), vmass)); // velocityX tidbit
store_pd(cloud->l1 + i, mul_pd(vdt, cloud->getVx1_pd(i))); // positionX tidbit
store_pd(cloud->m1 + i, div_pd(mul_pd(vdt, load_pd(pFy)), vmass)); // velocityY tidbit
store_pd(cloud->n1 + i, mul_pd(vdt, cloud->getVy1_pd(i))); // positionY tidbit
// reset forces to zero:
store_pd(pFx, set0_pd());
store_pd(pFy, set0_pd());
END_PARALLEL_FOR
operate2(currentTime + dt/2.0);
force2(currentTime + dt/2.0); // compute net force2
BEGIN_PARALLEL_FOR(i, e, numParticles, DOUBLE_STRIDE, static) // calculate k2 and l for entire cloud
const doubleV vmass = load_pd(cloud->mass + i);
// assign force pointers:
double * const pFx = cloud->forceX + i;
double * const pFy = cloud->forceY + i;
store_pd(cloud->k2 + i, div_pd(mul_pd(vdt, load_pd(pFx)), vmass)); // velocityX tidbit
store_pd(cloud->l2 + i, mul_pd(vdt, cloud->getVx2_pd(i))); // positionX tidbit
store_pd(cloud->m2 + i, div_pd(mul_pd(vdt, load_pd(pFy)), vmass)); // velocityY tidbit
store_pd(cloud->n2 + i, mul_pd(vdt, cloud->getVy2_pd(i))); // positionY tidbit
// reset forces to zero:
store_pd(pFx, set0_pd());
store_pd(pFy, set0_pd());
END_PARALLEL_FOR
// Calculate next position and next velocity for entire cloud.
BEGIN_PARALLEL_FOR(i, e, numParticles, DOUBLE_STRIDE, static)
plusEqual_pd(cloud->Vx + i, load_pd(cloud->k2 + i));
plusEqual_pd(cloud->x + i, load_pd(cloud->l2 + i));
plusEqual_pd(cloud->Vy + i, load_pd(cloud->m2 + i));
plusEqual_pd(cloud->y + i, load_pd(cloud->n2 + i));
END_PARALLEL_FOR
currentTime += dt;
}
}
vector<float> FastShift::allDirections(vector<float> shiftPoint, vector<float> shiftValue)
{
vector<int> grid_pos(n, 0);
whichGrid(shiftPoint, grid_pos);
int pos = findposition(grid_pos);
vector<float> external_force(n, 0);
vector<float> force1(n, 0);
for(int i = 0; i < selectD; i ++)
{
for(int j = -1; j <= 1; j += 2)
{
calForce(shiftPoint, pos + j * factor[selected[i]], force1);
external_force += force1;
}
}
return external_force;
}
vector<float> FastShift::maxDirectionTwoSide(vector<float> shiftPoint, vector<float> shiftValue)
{
vector<float> external_force(n, 0);
vector<float> force(n, 0);
vector<float> force1(n, 0);
bool unset = true;
float max;
int shift;
vector<int> grid_pos(n, 0);
whichGrid(shiftPoint, grid_pos);
int pos = findposition(grid_pos);
for(int i = 0; i < selectD; i ++)
{
for(int j = -1; j <= 1; j += 2)
{
calForce(shiftPoint, pos + j * factor[selected[i]], force1);
force += force1;
}
float l = l2norm(force);
if(unset)
{
max = l;
external_force = force;
shift = pos - factor[selected[i]];
unset = false;
continue;
}
if(l > max)
{
max = l;
external_force = force;
}
}
//copy(C[shift], shiftValue, n);
return external_force;
}
DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide)
{
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]);
dgSoaFloat force0(m_soaZero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = dgSoaFloat(body0->m_externalForce, body0->m_externalTorque);
}
dgSoaFloat force1(m_soaZero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = dgSoaFloat(body1->m_externalForce, 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);
}
}
const dgFloat32 forceImpulseScale = dgFloat32(1.0f);
const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
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));
dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal();
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));
dgFloat32 diag = tmpDiag.AddHorizontal();
dgAssert(diag > dgFloat32(0.0f));
rhs->m_diagDamp = diag * rhs->m_stiffness;
diag *= (dgFloat32(1.0f) + rhs->m_stiffness);
//rhs->m_jinvMJt = diag;
rhs->m_invJinvMJt = dgFloat32(1.0f) / diag;
}
}
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;
}
}
