void dgBilateralConstraint::SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 spring, dgFloat32 damper) { if (desc.m_timestep > dgFloat32 (0.0f)) { dgAssert (m_body1); const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; dgVector veloc0 (m_body0->m_veloc); dgVector omega0 (m_body0->m_omega); dgVector veloc1 (m_body1->m_veloc); dgVector omega1 (m_body1->m_omega); //dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle; dgFloat32 relPosit = desc.m_penetration[index]; dgFloat32 relVeloc = - (veloc0 % jacobian0.m_linear + veloc1 % jacobian1.m_linear + omega0 % jacobian0.m_angular + omega1 % jacobian1.m_angular); //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] dgFloat32 dt = desc.m_timestep; dgFloat32 ks = dgAbsf (spring); dgFloat32 kd = dgAbsf (damper); dgFloat32 ksd = dt * ks; dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc; dgFloat32 den = dt * kd + dt * ksd; dgFloat32 accel = num / (dgFloat32 (1.0f) + den); // desc.m_jointStiffness[index] = - den / DG_PSD_DAMP_TOL ; desc.m_jointStiffness[index] = - dgFloat32 (1.0f) - den / DG_PSD_DAMP_TOL; SetMotorAcceleration (index, accel, desc); } }
void Foam::backwardDiffusionFvPatchScalarField::write(Ostream& os) const { fvPatchScalarField::write(os); omega0().writeEntry("omega0", os); rho0().writeEntry("rho0", os); writeEntry("value", os); }
void rayPickerManager::PreUpdate(dFloat timestep) { // all of the work will be done here; dNewton::ScopeLock scopelock (&m_lock); if (m_pickedBody) { if (m_pickedBody->GetType() == dNewtonBody::m_dynamic) { newtonDynamicBody* const body = (newtonDynamicBody*)m_pickedBody; dFloat invTimeStep = 1.0f / timestep; Matrix matrix (body->GetMatrix()); Vec4 omega0 (body->GetOmega()); Vec4 veloc0 (body->GetVeloc()); Vec4 peekPosit (m_localpHandlePoint * matrix); Vec4 peekStep (m_globalTarget - peekPosit); Vec4 pointVeloc (body->GetPointVeloc (peekPosit)); Vec4 deltaVeloc (peekStep * (m_stiffness * invTimeStep) - pointVeloc); for (int i = 0; i < 3; i ++) { Vec4 veloc (0.0f, 0.0f, 0.0f, 0.0f); veloc[i] = deltaVeloc[i]; body->ApplyImpulseToDesiredPointVeloc (peekPosit, veloc); } // damp angular velocity Vec4 omega1 (body->GetOmega()); Vec4 veloc1 (body->GetVeloc()); omega1 = omega1 * (0.9f); // restore body velocity and angular velocity body->SetVeloc(veloc0); body->SetOmega(omega0); // convert the delta velocity change to a external force and torque dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; body->GetMassAndInertia (mass, Ixx, Iyy, Izz); matrix.setTrans(Vec3(0.0f, 0.0f, 0.0f)); Vec4 relOmega (omega1 - omega0); relOmega = matrix.preMult(relOmega); Vec4 angularMomentum (Ixx, Iyy, Izz, 0.0f); angularMomentum = componentMultiply (relOmega, angularMomentum); angularMomentum = matrix.postMult(angularMomentum); Vec4 torque (angularMomentum * invTimeStep); body->AddTorque(torque); Vec4 relVeloc (veloc1 - veloc0); Vec4 force (relVeloc * (mass * invTimeStep)); body->AddForce (force); } else { dAssert (0); } } }
dgFloat32 dgBilateralConstraint::CalculateSpringDamperAcceleration ( dgInt32 index, const dgContraintDescritor& desc, dgFloat32 jointAngle, const dgVector& p0Global, const dgVector& p1Global, dgFloat32 springK, dgFloat32 springD) { dgFloat32 accel = 0.0f; if (desc.m_timestep > dgFloat32 (0.0f)) { dgAssert (m_body1); const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; dgVector veloc0 (m_body0->m_veloc); dgVector omega0 (m_body0->m_omega); dgVector veloc1 (m_body1->m_veloc); dgVector omega1 (m_body1->m_omega); dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle; dgFloat32 relVeloc = - (veloc0 % jacobian0.m_linear + veloc1 % jacobian1.m_linear + omega0 % jacobian0.m_angular + omega1 % jacobian1.m_angular); //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] dgFloat32 dt = desc.m_timestep; dgFloat32 ks = springK; dgFloat32 kd = springD; dgFloat32 ksd = dt * ks; dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc; dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd; accel = num / den; } return accel; }
Foam::backwardDiffusionFvPatchScalarField::backwardDiffusionFvPatchScalarField ( const fvPatch& p, const DimensionedField<scalar, volMesh>& iF ) : mixedUserDefinedFvPatchScalarField(p, iF) { alfa() = 0.0; beta() = 0.0; eta() = 0.0; omega0() = 0.0; rho0() = 0.0; epsilon() = 0.0; nameInternal_ = dimensionedInternalField().name(); }
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep) { const dFloat angleError = GetMaxAngleError(); dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up); NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); if (dAbs(angle0) > angleError) { const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep); NewtonUserJointSetRowAcceleration(m_joint, alpha); } dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right); NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); if (dAbs(angle1) > angleError) { const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep); NewtonUserJointSetRowAcceleration(m_joint, alpha); } // the joint angle can be determined by getting the angle between any two non parallel vectors m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front)); // save the current joint Omega dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front); if (m_options.m_option2) { if (m_options.m_option3) { dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep); } else { dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep); } } else if (m_options.m_option3) { dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep); } else if (m_angularFriction != 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep); NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction); } }
void dCustomHinge::SubmitConstraints(dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; dFloat sinAngle; dFloat cosAngle; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix(matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); // two rows to restrict rotation around around the parent coordinate system NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); // the joint angle can be determined by getting the angle between any two non parallel vectors CalculateAngle (matrix1.m_up, matrix0.m_up, matrix1.m_front, sinAngle, cosAngle); m_curJointAngle.Update(cosAngle, sinAngle); // save the current joint Omega dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_jointOmega = (omega0 - omega1).DotProduct3(matrix1.m_front); m_lastRowWasUsed = false; if (m_setAsSpringDamper) { ApplySpringDamper (timestep, matrix0, matrix1); } else { SubmitConstraintsFreeDof (timestep, matrix0, matrix1); } }
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep) { dMatrix localMatrix(matrix0 * matrix1.Inverse()); dVector euler0; dVector euler1; localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw); dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f)); rollPin = matrix1.RotateVector(rollPin); NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); // the joint angle can be determined by getting the angle between any two non parallel vectors m_curJointAngle.Update(euler0.m_x); // save the current joint Omega dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front); if (m_options.m_option2) { if (m_options.m_option3) { dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep); } else { dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep); } } else if (m_options.m_option3) { dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep); } else if (m_angularFriction != 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep); NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction); } }
void CustomBallAndSocketWithFriction::SubmitConstraints(dFloat timestep, int threadIndex) { CustomBallAndSocket::SubmitConstraints(timestep, threadIndex); dVector omega0(0.0f, 0.0f, 0.0f, 0.0f); dVector omega1(0.0f, 0.0f, 0.0f, 0.0f); // get the omega vector NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } dVector relOmega(omega0 - omega1); dFloat omegaMag = dSqrt(relOmega % relOmega); if (omegaMag > 0.1f) { // tell newton to used this the friction of the omega vector to apply the rolling friction dMatrix basis(dGrammSchmidt(relOmega)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]); NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]); NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction); // calculate the acceleration to stop the ball in one time step dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f; NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[0][0]); NewtonUserJointSetRowAcceleration(m_joint, -omegaMag * invTimestep); NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction); } else { // when omega is too low this is correct but the small angle approximation theorem. dMatrix basis(dGetIdentityMatrix()); for (int i = 0; i < 3; i++) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[i][0]); NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction); } } }
Foam::backwardDiffusionFvPatchScalarField::backwardDiffusionFvPatchScalarField ( const fvPatch& p, const DimensionedField<scalar, volMesh>& iF, const dictionary& dict ) : mixedUserDefinedFvPatchScalarField(p, iF) { // Name of field nameInternal_ = dimensionedInternalField().name(); // Set the nominal value omega0() = scalarField("omega0", dict, p.size()); rho0() = scalarField("rho0", dict, p.size()); // Fixed value condition is forced alfa() = 1000.; eta() = 1.; // Calculating epsilon epsilon() = 0; // Calculating beta const double Dmix = 1e-10; beta() = Dmix*this->patch().deltaCoeffs(); // Read value if available if (dict.found("value")) { fvPatchField<scalar>::operator= ( scalarField("value", dict, p.size()) ); } else { evaluate(); } }
void dCustomRackAndPinion::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; dVector omega0(0.0f); dVector veloc1(0.0f); dFloat jacobian0[6]; dFloat jacobian1[6]; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // calculate the angular velocity for both bodies NewtonBodyGetOmega(m_body0, &omega0[0]); NewtonBodyGetVelocity(m_body1, &veloc1[0]); dVector dir0 (matrix0.m_front.Scale (m_gearRatio)); const dVector& dir1 = matrix1.m_front; jacobian0[0] = dFloat(0.0f); jacobian0[1] = dFloat(0.0f); jacobian0[2] = dFloat(0.0f); jacobian0[3] = dir0.m_x; jacobian0[4] = dir0.m_y; jacobian0[5] = dir0.m_z; jacobian1[0] = dir1.m_x; jacobian1[1] = dir1.m_y; jacobian1[2] = dir1.m_z; jacobian1[3] = dFloat(0.0f); jacobian1[4] = dFloat(0.0f); jacobian1[5] = dFloat(0.0f); dFloat w0 = omega0.DotProduct3(dir0); dFloat w1 = veloc1.DotProduct3(dir1); dFloat relOmega = w0 + w1; dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f; dFloat relAccel = -0.5f * relOmega * invTimestep; NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1); NewtonUserJointSetRowAcceleration (m_joint, relAccel); }
void dCustomHinge::SubmitConstraintsFreeDof(int freeDof, const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep, int threadIndex) { dAssert (freeDof == 1); dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_jointOmega = (omega0 - omega1).DotProduct3(matrix1.m_front); //m_friction = 0; //m_limitsOn = false; //m_setAsSpringDamper = 1; //m_spring = 1000; //m_damper = 10.0f; //m_springDamperRelaxation = 0.97f; if (m_setAsSpringDamper) { //m_lastRowWasUsed = true; NewtonUserJointAddAngularRow(m_joint, -GetPitch(), &matrix0.m_front[0]); NewtonUserJointSetRowSpringDamperAcceleration(m_joint, m_springDamperRelaxation, m_spring, m_damper); } else { if (m_limitsOn) { if (m_friction != 0.0f) { SubmitConstraintsFrictionAndLimit(matrix0, matrix1, timestep); } else { SubmitConstraintsLimitsOnly(matrix0, matrix1, timestep); } } else { if (m_friction != 0.0f) { SubmitConstraintsFrictionOnly(matrix0, matrix1, timestep); } } } }
void dCustomBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix(matrix0, matrix1); SubmitLinearRows(0x07, matrix0, matrix1); const dVector& coneDir0 = matrix0.m_front; const dVector& coneDir1 = matrix1.m_front; dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0); dMatrix coneRotation(dGetIdentityMatrix()); dVector lateralDir(matrix0.m_up); if (cosAngleCos < 0.9999f) { lateralDir = coneDir1.CrossProduct(coneDir0); dFloat mag2 = lateralDir.DotProduct3(lateralDir); if (mag2 > 1.0e-4f) { lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2)); coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit); } else { lateralDir = matrix0.m_up.Scale (-1.0f); coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit); } } dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } dVector relOmega(omega0 - omega1); // do twist angle calculations dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse()); dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1])); if (m_options.m_option0) { if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) { NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); } else { if (m_options.m_option1) { // TODO spring option dAssert (0); } else { SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep); } } } else if (m_options.m_option1) { // TODO spring option dAssert (0); } else if (m_twistFriction > 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction); } // do twist cone angle calculations if (m_options.m_option2) { if ((m_maxConeAngle == 0.0f)) { dMatrix localMatrix(matrix0 * matrix1.Inverse()); dVector euler0; dVector euler1; localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw); NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); } else { if (m_options.m_option3) { // TODO spring option dAssert(0); } else { dFloat jointOmega = relOmega.DotProduct3(lateralDir); dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f))); dFloat coneAngle = currentAngle + jointOmega * timestep; if (coneAngle >= m_maxConeAngle) { //dQuaternion rot(lateralDir, coneAngle); //dVector frontDir(rot.RotateVector(coneDir1)); //dVector upDir(lateralDir.CrossProduct(frontDir)); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); const dFloat invtimestep = 1.0f / timestep; const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep; const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep; NewtonUserJointSetRowAcceleration(m_joint, stopAccel); } else if (m_coneFriction != 0) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); } } } } else if (m_options.m_option3) { // TODO spring option dAssert(0); } else if (m_coneFriction > 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); } }
void dgBilateralConstraint::CalculateAngularDerivative ( dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, dgFloat32 stiffness, dgFloat32 jointAngle, dgFloat32* jointForce) { dgFloat32 alphaError; dgFloat32 omegaError; _ASSERTE (jointForce); dgVector omega1; _ASSERTE (m_body0); dgVector omega0 (m_body0->GetOmega()); dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobian_IM0; jacobian0.m_linear[0] = dgFloat32 (0.0f); jacobian0.m_linear[1] = dgFloat32 (0.0f); jacobian0.m_linear[2] = dgFloat32 (0.0f); jacobian0.m_linear[3] = dgFloat32 (0.0f); jacobian0.m_angular[0] = dir.m_x; jacobian0.m_angular[1] = dir.m_y; jacobian0.m_angular[2] = dir.m_z; jacobian0.m_angular[3] = dgFloat32 (0.0f); dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobian_IM1; _ASSERTE (m_body1); omega1 = m_body1->GetOmega(); jacobian1.m_linear[0] = dgFloat32 (0.0f); jacobian1.m_linear[1] = dgFloat32 (0.0f); jacobian1.m_linear[2] = dgFloat32 (0.0f); jacobian1.m_linear[3] = dgFloat32 (0.0f); jacobian1.m_angular[0] = -dir.m_x; jacobian1.m_angular[1] = -dir.m_y; jacobian1.m_angular[2] = -dir.m_z; jacobian1.m_angular[3] = dgFloat32 (0.0f); omegaError = (omega1 - omega0) % dir; //at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] dgFloat32 dt = desc.m_timestep; dgFloat32 ks = DG_POS_DAMP; dgFloat32 kd = DG_VEL_DAMP; dgFloat32 ksd = dt * ks; dgFloat32 num = ks * jointAngle + kd * omegaError + ksd * omegaError; dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd; alphaError = num / den; m_rowIsMotor[index] = 0; desc.m_isMotor[index] = 0; m_motorAcceleration[index] = dgFloat32 (0.0f); //_ASSERTE (dgAbsf (alphaError - CalculateSpringDamperAcceleration (index, desc, jointAngle, dgVector (0, 0, 0), dgVector (0, 0, 0), ANGULAR_POS_DAMP, ANGULAR_VEL_DAMP)) < 1.0e-2f); desc.m_penetration[index] = jointAngle; desc.m_jointAccel[index] = alphaError; desc.m_restitution[index] = dgFloat32 (0.0f); desc.m_jointStiffness[index] = stiffness; desc.m_penetrationStiffness[index] = dgFloat32 (0.0f); desc.m_forceBounds[index].m_jointForce = jointForce; }
void MSNewton::Servo::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); ServoData* cj_data = (ServoData*)joint_data->cj_data; // Calculate position of pivot points and Jacobian direction vectors in global space. dMatrix matrix0, matrix1, matrix2; MSNewton::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1, matrix2); // Calculate angle, omega, and acceleration. dFloat last_angle = cj_data->ai->get_angle(); dFloat last_omega = cj_data->cur_omega; dFloat sin_angle; dFloat cos_angle; Joint::c_calculate_angle(matrix1.m_front, matrix0.m_front, matrix0.m_right, sin_angle, cos_angle); cj_data->ai->update(cos_angle, sin_angle); cj_data->cur_omega = (cj_data->ai->get_angle() - last_angle) / timestep; cj_data->cur_accel = (cj_data->cur_omega - last_omega) / timestep; dFloat cur_angle = cj_data->ai->get_angle(); // Restrict movement on the pivot point along all tree orthonormal directions. NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); // Add two rows to restrict rotation around the the axis perpendicular to the rotation axis. /*NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_front), &matrix0.m_front[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_up), &matrix0.m_up[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);*/ // Add two more rows to achieve a more robust angular constraint. // Get a point along the pin axis at some reasonable large distance from the pivot. dVector q0(matrix0.m_posit + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1(matrix1.m_posit + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH)); // Add two constraints row perpendicular to the pin vector. NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix1.m_front[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix1.m_up[0]); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); // Add limits and friction if (cj_data->limits_enabled == true && cur_angle < cj_data->min - Joint::ANGULAR_LIMIT_EPSILON) { dFloat rel_angle = cj_data->min - cur_angle; NewtonUserJointAddAngularRow(joint, rel_angle, &matrix0.m_right[0]); NewtonUserJointSetRowMinimumFriction(joint, 0.0f); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } else if (cj_data->limits_enabled == true && cur_angle > cj_data->max + Joint::ANGULAR_LIMIT_EPSILON) { dFloat rel_angle = cj_data->max - cur_angle; NewtonUserJointAddAngularRow(joint, rel_angle, &matrix0.m_right[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); if (joint_data->ctype == CT_FLEXIBLE) NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP); else if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } else { if (cj_data->controller_enabled) { // Get relative angular velocity dVector omega0(0.0f, 0.0f, 0.0f); dVector omega1(0.0f, 0.0f, 0.0f); NewtonBodyGetOmega(joint_data->child, &omega0[0]); if (joint_data->parent != nullptr) NewtonBodyGetOmega(joint_data->parent, &omega1[0]); dFloat rel_omega = (omega0 - omega1) % matrix1.m_right; // Calculate relative angle dFloat desired_angle = cj_data->limits_enabled ? Util::clamp(cj_data->controller, cj_data->min, cj_data->max) : cj_data->controller; dFloat rel_angle = desired_angle - cur_angle; dFloat arel_angle = dAbs(rel_angle); // Calculate desired accel dFloat mar = cj_data->rate * cj_data->reduction_ratio; dFloat ratio = (cj_data->rate > EPSILON && cj_data->reduction_ratio > EPSILON && arel_angle < mar) ? arel_angle / mar : 1.0f; dFloat step = cj_data->rate * ratio * dSign(rel_angle) * timestep; if (dAbs(step) > arel_angle) step = rel_angle; dFloat desired_omega = step / timestep; dFloat desired_accel = (desired_omega - rel_omega) / timestep; // Add angular row NewtonUserJointAddAngularRow(joint, step, &matrix0.m_right[0]); // Apply acceleration NewtonUserJointSetRowAcceleration(joint, desired_accel); } else { // Add angular row NewtonUserJointAddAngularRow(joint, 0.0f, &matrix1.m_right[0]); } if (cj_data->power == 0.0f) { NewtonUserJointSetRowMinimumFriction(joint, -Joint::CUSTOM_LARGE_VALUE); NewtonUserJointSetRowMaximumFriction(joint, Joint::CUSTOM_LARGE_VALUE); } else { NewtonUserJointSetRowMinimumFriction(joint, -cj_data->power); NewtonUserJointSetRowMaximumFriction(joint, cj_data->power); } NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } }
void CustomCorkScrew::SubmitConstrainst () { dFloat dist; dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction dVector p0 (matrix0.m_posit); dVector p1 (matrix1.m_posit + matrix1.m_front.Scale ((p0 - matrix1.m_posit) % matrix1.m_front)); NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]); // get a point along the pin axis at some reasonable large distance from the pivot dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); // two constraints row perpendiculars to the hinge pin NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]); // if limit are enable ... if (m_limitsOn) { dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front; if (dist < m_minDist) { // get a point along the up vector and set a constraint NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]); // allow the object to return but not to kick going forward NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } else if (dist > m_maxDist) { // get a point along the up vector and set a constraint NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]); // allow the object to return but not to kick going forward NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } } if (m_angularmotorOn) { dFloat relOmega; dFloat relAccel; dVector omega0 (0.0f, 0.0f, 0.0f); dVector omega1 (0.0f, 0.0f, 0.0f); // get relative angular velocity NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } // calculate the desired acceleration relOmega = (omega0 - omega1) % matrix0.m_front; relAccel = m_angularAccel - m_angularDamp * relOmega; // if the motor capability is on, then set angular acceleration with zero angular correction NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); // override the angular acceleration for this Jacobian to the desired acceleration NewtonUserJointSetRowAcceleration (m_joint, relAccel); } }
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); // construct an orthogonal coordinate system with these two vectors dMatrix matrix1_1; matrix1_1.m_up = matrix1.m_up; matrix1_1.m_right = matrix0.m_front * matrix1.m_up; matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right)); matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right; NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]); dFloat sinAngle_0; dFloat cosAngle_0; CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0); dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0); dFloat sinAngle_1; dFloat cosAngle_1; CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1); dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1); dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f); dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } // calculate the desired acceleration dVector relOmega (omega0 - omega1); m_jointOmega_0 = relOmega % matrix0.m_front; m_jointOmega_1 = relOmega % matrix1.m_up; // check is the joint limit are enable if (m_limit_0_On) { if (angle0 < m_minAngle_0) { dFloat relAngle = angle0 - m_minAngle_0; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffeners here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (angle0 > m_maxAngle_0) { dFloat relAngle = angle0 - m_maxAngle_0; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } // check is the joint limit motor is enable } else if (m_angularMotor_0_On) { // calculate the desired acceleration // dFloat relOmega = (omega0 - omega1) % matrix0.m_front; dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0; // add and angular constraint row to that will set the relative acceleration to zero NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); // override the joint acceleration. NewtonUserJointSetRowAcceleration (m_joint, relAccel); } // if limit are enable ... if (m_limit_1_On) { if (angle1 < m_minAngle_1) { dFloat relAngle = angle1 - m_minAngle_1; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]); // need high stiffeners here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (angle1 > m_maxAngle_1) { dFloat relAngle = angle1 - m_maxAngle_1; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } else if (m_angularMotor_1_On) { // calculate the desired acceleration dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1; // add and angular constraint row to that will set the relative acceleration to zero NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]); // override the joint acceleration. NewtonUserJointSetRowAcceleration (m_joint, relAccel); } }
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep) { dMatrix matrix; dVector com(0.0f); dVector omega0(0.0f); dVector veloc0(0.0f); dVector omega1(0.0f); dVector veloc1(0.0f); dVector pointVeloc(0.0f); const dFloat stiffness = 0.3f; const dFloat angularDamp = 0.95f; dFloat invTimeStep = 1.0f / timestep; NewtonWorld* const world = NewtonBodyGetWorld (body); NewtonWorldCriticalSectionLock (world, 0); // calculate the desired impulse NewtonBodyGetMatrix(body, &matrix[0][0]); NewtonBodyGetOmega (body, &omega0[0]); NewtonBodyGetVelocity (body, &veloc0[0]); NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]); dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace); deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc; for (int i = 0; i < 3; i ++) { dVector veloc (0.0f); veloc[i] = deltaVeloc[i]; NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]); } // damp angular velocity NewtonBodyGetOmega (body, &omega1[0]); NewtonBodyGetVelocity (body, &veloc1[0]); omega1 = omega1.Scale (angularDamp); // restore body velocity and angular velocity NewtonBodySetOmega (body, &omega0[0]); NewtonBodySetVelocity(body, &veloc0[0]); // convert the delta velocity change to a external force and torque dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz); dVector angularMomentum (Ixx, Iyy, Izz); angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0))); dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep)); dVector torque (angularMomentum.Scale(invTimeStep)); NewtonBodyAddForce(body, &force[0]); NewtonBodyAddTorque(body, &torque[0]); // make sure the body is unfrozen, if it is picked NewtonBodySetSleepState (body, 0); NewtonWorldCriticalSectionUnlock (world); }
void CustomHinge::SubmitConstraints (dFloat timestep, int threadIndex) { // dFloat angle; // dFloat sinAngle; // dFloat cosAngle; dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]); // get a point along the pin axis at some reasonable large distance from the pivot dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); // two constraints row perpendicular to the pin vector NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]); // the joint angle can be determine by getting the angle between any two non parallel vectors dFloat angle; dFloat sinAngle; dFloat cosAngle; sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front; cosAngle = matrix0.m_up % matrix1.m_up; angle = m_curJointAngle.CalculateJointAngle (cosAngle, sinAngle); // if limit are enable ... if (m_limitsOn) { // the joint angle can be determine by getting the angle between any two non parallel vectors // sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front; // cosAngle = matrix0.m_up % matrix1.m_up; // angle = dAtan2 (sinAngle, cosAngle); // if (angle < m_minAngle) { if (angle < m_minAngle) { dFloat relAngle; // relAngle = angle - m_minAngle; relAngle = angle - m_minAngle; // the angle was clipped save the new clip limit m_curJointAngle.m_angle = m_minAngle; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (angle > m_maxAngle) { dFloat relAngle; // relAngle = angle - m_maxAngle; relAngle = angle - m_maxAngle; // the angle was clipped save the new clip limit m_curJointAngle.m_angle = m_maxAngle; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } // save the current joint Omega dVector omega0(0.0f, 0.0f, 0.0f, 0.0f); dVector omega1(0.0f, 0.0f, 0.0f, 0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_jointOmega = (omega0 - omega1) % matrix0.m_front; }
void CustomCorkScrew::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]); // two rows to restrict rotation around around the parent coordinate system dFloat sinAngle; dFloat cosAngle; CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle); NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]); CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle); NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]); // if limit are enable ... if (m_limitsLinearOn) { dFloat dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front; if (dist < m_minLinearDist) { // get a point along the up vector and set a constraint NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]); // allow the object to return but not to kick going forward NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } else if (dist > m_maxLinearDist) { // get a point along the up vector and set a constraint NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]); // allow the object to return but not to kick going forward NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } } CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle); dFloat angle = -m_curJointAngle.Update (cosAngle, sinAngle); if (m_limitsAngularOn) { // the joint angle can be determine by getting the angle between any two non parallel vectors if (angle < m_minAngularDist) { dFloat relAngle = angle - m_minAngularDist; // the angle was clipped save the new clip limit //m_curJointAngle.m_angle = m_minAngularDist; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (angle > m_maxAngularDist) { dFloat relAngle = angle - m_maxAngularDist; // the angle was clipped save the new clip limit //m_curJointAngle.m_angle = m_maxAngularDist; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } if (m_angularmotorOn) { dVector omega0 (0.0f, 0.0f, 0.0f); dVector omega1 (0.0f, 0.0f, 0.0f); // get relative angular velocity NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } // calculate the desired acceleration dFloat relOmega = (omega0 - omega1) % matrix0.m_front; dFloat relAccel = m_angularAccel - m_angularDamp * relOmega; // if the motor capability is on, then set angular acceleration with zero angular correction NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); // override the angular acceleration for this Jacobian to the desired acceleration NewtonUserJointSetRowAcceleration (m_joint, relAccel); } }
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex) { dFloat angle; dFloat sinAngle; dFloat cosAngle; dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1); // get the pin fixed to the first body const dVector& dir0 = matrix0.m_front; // get the pin fixed to the second body const dVector& dir1 = matrix1.m_up; // construct an orthogonal coordinate system with these two vectors dVector dir2 (dir0 * dir1); dir2 = dir2.Scale (1.0f / dSqrt (dir2 % dir2)); const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix1.m_posit; NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir0[0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir1[0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir2[0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); dVector dir3 (dir2 * dir0); dVector q0 (p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1 (p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH)); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &dir0[0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // check is the joint limit are enable if (m_limit_0_On) { sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front; cosAngle = matrix0.m_up % matrix1.m_up; angle = dAtan2 (sinAngle, cosAngle); if (angle < m_minAngle_0) { dFloat relAngle; relAngle = angle - m_minAngle_0; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffeners here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (angle > m_maxAngle_0) { dFloat relAngle; relAngle = angle - m_maxAngle_0; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } // check is the joint limit motor is enable } else if (m_angularMotor_0_On) { dFloat relOmega; dFloat relAccel; dVector omega0 (0.0f, 0.0f, 0.0f); dVector omega1 (0.0f, 0.0f, 0.0f); // get relative angular velocity NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } // calculate the desired acceleration relOmega = (omega0 - omega1) % matrix0.m_front; relAccel = m_angularAccel_0 - m_angularDamp_0 * relOmega; // add and angular constraint row to that will set the relative acceleration to zero NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); // override the joint acceleration. NewtonUserJointSetRowAcceleration (m_joint, relAccel); } // if limit are enable ... if (m_limit_1_On) { sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up; cosAngle = matrix0.m_front % matrix1.m_front; angle = dAtan2 (sinAngle, cosAngle); if (angle < m_minAngle_1) { dFloat relAngle; relAngle = angle - m_minAngle_1; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]); // need high stiffeners here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (angle > m_maxAngle_1) { dFloat relAngle; relAngle = angle - m_maxAngle_1; // tell joint error will minimize the exceeded angle error NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]); // need high stiffness here NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the joint to move back freely NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } else if (m_angularMotor_1_On) { dFloat relOmega; dFloat relAccel; dVector omega0 (0.0f, 0.0f, 0.0f); dVector omega1 (0.0f, 0.0f, 0.0f); // get relative angular velocity NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } // calculate the desired acceleration relOmega = (omega0 - omega1) % matrix1.m_up; relAccel = m_angularAccel_1 - m_angularDamp_1 * relOmega; // add and angular constraint row to that will set the relative acceleration to zero NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]); // override the joint acceleration. NewtonUserJointSetRowAcceleration (m_joint, relAccel); } }