void MSNewton::Slider::get_info(const NewtonJoint* const joint, NewtonJointRecord* const info) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); SliderData* cj_data = (SliderData*)joint_data->cj_data; info->m_minLinearDof[0] = -0.0f; info->m_maxLinearDof[0] = 0.0f; info->m_minLinearDof[1] = -0.0f; info->m_maxLinearDof[1] = 0.0f; if (cj_data->limits_enabled) { info->m_minLinearDof[2] = (cj_data->min - cj_data->cur_pos); info->m_minLinearDof[2] = (cj_data->max - cj_data->cur_pos); } else { info->m_minLinearDof[2] = -Joint::CUSTOM_LARGE_VALUE; info->m_minLinearDof[2] = Joint::CUSTOM_LARGE_VALUE; } info->m_minAngularDof[0] = -0.0f; info->m_maxAngularDof[0] = 0.0f; info->m_minAngularDof[1] = -0.0f; info->m_maxAngularDof[1] = 0.0f; info->m_minAngularDof[2] = -0.0f; info->m_maxAngularDof[2] = 0.0f; }
void MSNewton::Fixed::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); // 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); const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix1.m_posit; // Get a point along the pin axis at some reasonable large distance from the pivot. dVector q0(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1(p1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH)); // Get the ankle point. dVector r0(p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector r1(p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); // Restrict movement on the pivot point along all three orthonormal directions NewtonUserJointAddLinearRow(joint, &p0[0], &p1[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, &p0[0], &p1[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, &p0[0], &p1[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); // Restrict rotation along all three orthonormal directions NewtonUserJointAddLinearRow(joint, &q0[0], &q1[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, &q0[0], &q1[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, &r0[0], &r1[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); }
void MSNewton::Servo::get_info(const NewtonJoint* const joint, NewtonJointRecord* const info) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); ServoData* cj_data = (ServoData*)joint_data->cj_data; info->m_minLinearDof[0] = -0.0f; info->m_maxLinearDof[0] = 0.0f; info->m_minLinearDof[1] = -0.0f; info->m_maxLinearDof[1] = 0.0f; info->m_minLinearDof[2] = -0.0f; info->m_maxLinearDof[2] = 0.0f; info->m_minAngularDof[0] = -0.0f; info->m_maxAngularDof[0] = 0.0f; info->m_minAngularDof[1] = -0.0f; info->m_maxAngularDof[1] = 0.0f; if (cj_data->limits_enabled) { info->m_minAngularDof[2] = (cj_data->min - cj_data->ai->get_angle()) * RAD_TO_DEG; info->m_maxAngularDof[2] = (cj_data->max - cj_data->ai->get_angle()) * RAD_TO_DEG; } else { info->m_minAngularDof[2] = -Joint::CUSTOM_LARGE_VALUE; info->m_maxAngularDof[2] = Joint::CUSTOM_LARGE_VALUE; } }
unsigned cPhysicsJointHingeNewton::LimitCallback(const NewtonJoint* pHinge, NewtonHingeSliderUpdateDesc* pDesc) { cPhysicsJointHingeNewton* pHingeJoint = (cPhysicsJointHingeNewton*)NewtonJointGetUserData(pHinge); //pHingeJoint->OnPhysicsUpdate(); if(pHingeJoint->mfMaxAngle == 0 && pHingeJoint->mfMinAngle == 0) return 0; float fAngle = NewtonHingeGetJointAngle (pHinge); //Avoid oscillation CheckLimitAutoSleep(pHingeJoint, pHingeJoint->mfMinAngle,pHingeJoint->mfMaxAngle,fAngle); bool bSkipLimitCheck = false; if(fabs(pHingeJoint->mfPreviousAngle - fAngle) > cMath::ToRad(300)) bSkipLimitCheck = true; //Max limit if (fAngle > pHingeJoint->mfMaxAngle && bSkipLimitCheck==false) { pHingeJoint->OnMaxLimit(); pDesc->m_accel = NewtonHingeCalculateStopAlpha (pHinge, pDesc, pHingeJoint->mfMaxAngle); pDesc->m_maxFriction =0; pHingeJoint->mfPreviousAngle = fAngle; return 1; } //Min limit else if (fAngle < pHingeJoint->mfMinAngle && bSkipLimitCheck==false) { pHingeJoint->OnMinLimit(); pDesc->m_accel = NewtonHingeCalculateStopAlpha (pHinge, pDesc, pHingeJoint->mfMinAngle); pDesc->m_minFriction =0; pHingeJoint->mfPreviousAngle = fAngle; return 1; } else { if(pHingeJoint->mpParentBody ==NULL || pHingeJoint->mpParentBody->GetMass()==0) { if( (pHingeJoint->mfStickyMaxDistance != 0 && fabs(fAngle - pHingeJoint->mfMaxAngle) < pHingeJoint->mfStickyMaxDistance) || (pHingeJoint->mfStickyMinDistance != 0 && fabs(fAngle - pHingeJoint->mfMinAngle) < pHingeJoint->mfStickyMinDistance) ) { pHingeJoint->mpChildBody->SetAngularVelocity(0); pHingeJoint->mpChildBody->SetLinearVelocity(0); } } pHingeJoint->OnNoLimit(); } pHingeJoint->mfPreviousAngle = fAngle; return 0; }
void NewtonCustomJoint::SubmitConstrainst (const NewtonJoint* me) { NewtonCustomJoint* joint; // get the pointer to the joint class joint = (NewtonCustomJoint*) NewtonJointGetUserData (me); joint->SubmitConstrainst(); }
void SubmitConstraints(const void* const joint, float64 timestep, int threadIndex) { iPhysicsJoint* physicsJoint = static_cast<iPhysicsJoint*>(NewtonJointGetUserData(static_cast<const NewtonJoint*>(joint))); if (physicsJoint != nullptr) { physicsJoint->submitConstraints(static_cast<float64>(timestep), threadIndex); } }
void CustomDGRayCastCar::IntegrateTires (const NewtonJoint* userJoint, dFloat timestep, int threadIndex) { CustomDGRayCastCar* joint; // get the pointer to the joint class joint = (CustomDGRayCastCar*) NewtonJointGetUserData (userJoint); joint->IntegrateTires(timestep, threadIndex); }
void MSP::PointToPoint::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) { MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint)); PointToPointData* cj_data = reinterpret_cast<PointToPointData*>(joint_data->m_cj_data); dFloat inv_timestep = 1.0f / timestep; dMatrix matrix0, matrix1; MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1); dVector p0(matrix0.m_posit + matrix0.m_right.Scale(cj_data->m_start_distance)); const dVector& p1 = matrix1.m_posit; dVector veloc0(0.0f); dVector veloc1(0.0f); NewtonBodyGetVelocity(joint_data->m_child, &veloc0[0]); if (joint_data->m_parent != nullptr) NewtonBodyGetVelocity(joint_data->m_parent, &veloc1[0]); dVector rel_veloc(veloc0 - veloc1); dFloat stiffness = 0.999f - (1.0f - joint_data->m_stiffness_ratio * cj_data->m_strength) * Joint::DEFAULT_STIFFNESS_RANGE; if (cj_data->m_mode == 0) { dVector dir(p0 - p1); cj_data->m_cur_distance = Util::get_vector_magnitude(dir); if (cj_data->m_cur_distance > M_EPSILON) Util::scale_vector(dir, 1.0f / cj_data->m_cur_distance); else { dir.m_x = 0.0f; dir.m_y = 0.0f; dir.m_z = 1.0f; } dFloat offset = cj_data->m_cur_distance - cj_data->m_start_distance * cj_data->m_controller; dFloat dir_veloc = rel_veloc.DotProduct3(dir); dFloat des_accel = cj_data->m_accel * -offset - dir_veloc * inv_timestep * cj_data->m_damp; NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &dir[0]); NewtonUserJointSetRowAcceleration(joint, des_accel); NewtonUserJointSetRowStiffness(joint, stiffness); } else { dVector offset(matrix1.UntransformVector(p0)); cj_data->m_cur_distance = Util::get_vector_magnitude(offset); offset.m_z -= cj_data->m_start_distance * cj_data->m_controller; dVector loc_vel(matrix1.UnrotateVector(rel_veloc)); dVector des_accel(offset.Scale(-cj_data->m_accel) - loc_vel.Scale(inv_timestep * cj_data->m_damp)); NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_front[0]); NewtonUserJointSetRowAcceleration(joint, des_accel.m_x); NewtonUserJointSetRowStiffness(joint, stiffness); NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_up[0]); NewtonUserJointSetRowAcceleration(joint, des_accel.m_y); NewtonUserJointSetRowStiffness(joint, stiffness); NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_right[0]); NewtonUserJointSetRowAcceleration(joint, des_accel.m_z); NewtonUserJointSetRowStiffness(joint, stiffness); } }
void CustomJoint::Destructor (const NewtonJoint* me) { // get the pointer to the joint class CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (me); joint->m_autoDestroy = 1; // delete the joint class delete joint; }
void GetConnectedBodiesByJoints (NewtonBody* const body) { for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) { CustomJoint* const customJoint = (CustomJoint*) NewtonJointGetUserData(joint); NewtonBody* const body0 = customJoint->GetBody0(); NewtonBody* const body1 = customJoint->GetBody1(); NewtonBody* const otherBody = (body0 == body) ? body1 : body0; // do whatever you need to do here NewtonBodySetFreezeState (otherBody, 0); } }
void dCustomJoint::SubmitConstraints (const NewtonJoint* const me, dFloat timestep, int threadIndex) { // get the pointer to the joint class if (timestep != 0.0f) { dCustomJoint* const joint = (dCustomJoint*) NewtonJointGetUserData (me); // call the constraint call back if (joint) { joint->SubmitConstraints(timestep, threadIndex); } } }
unsigned cPhysicsJointScrewNewton::LimitCallback(const NewtonJoint* pScrew, NewtonHingeSliderUpdateDesc* pDesc) { cPhysicsJointScrewNewton* pScrewJoint = (cPhysicsJointScrewNewton*)NewtonJointGetUserData(pScrew); //pScrewJoint->OnPhysicsUpdate(); float fDistance = NewtonCorkscrewGetJointPosit (pScrew); //Log("Dist: %f\n",fDistance); if(pScrewJoint->mfMinDistance == 0 && pScrewJoint->mfMaxDistance == 0) return 0; //Avoid oscillation CheckLimitAutoSleep(pScrewJoint, pScrewJoint->mfMinDistance,pScrewJoint->mfMaxDistance,fDistance); if (fDistance < pScrewJoint->mfMinDistance) { pScrewJoint->OnMinLimit(); pDesc->m_accel = NewtonCorkscrewCalculateStopAccel (pScrew, pDesc, pScrewJoint->mfMinDistance); pDesc->m_minFriction =0; return 1; } else if (fDistance > pScrewJoint->mfMaxDistance) { pScrewJoint->OnMaxLimit(); pDesc->m_accel = NewtonCorkscrewCalculateStopAccel (pScrew, pDesc, pScrewJoint->mfMaxDistance); pDesc->m_maxFriction =0; return 1; } else { if(pScrewJoint->mpParentBody ==NULL || pScrewJoint->mpParentBody->GetMass()==0) { if( (pScrewJoint->mfStickyMaxDistance != 0 && fabs(fDistance - pScrewJoint->mfMaxDistance) < pScrewJoint->mfStickyMaxDistance) || (pScrewJoint->mfStickyMinDistance != 0 && fabs(fDistance - pScrewJoint->mfMinDistance) < pScrewJoint->mfStickyMinDistance) ) { pScrewJoint->mpChildBody->SetAngularVelocity(0); pScrewJoint->mpChildBody->SetLinearVelocity(0); } } pScrewJoint->OnNoLimit(); } return 0; }
static void SetTransform (const NewtonBody* body, const dFloat* matrix, int threadId) { NewtonUserJoint* player; PlayerController* controller; // find the player joint; player = NULL; for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) { NewtonUserJoint* tmp; tmp = (NewtonUserJoint*) NewtonJointGetUserData(joint); if (CustomGetJointID (tmp) == PLAYER_JOINT_ID) { player = tmp; break; } } // call the generic transform callback controller = (PlayerController*) CustomGetUserData(player); #if 1 // this will project the visual mesh to the ground dMatrix visualMatrix; CustomPlayerControllerGetVisualMaTrix (player, &visualMatrix[0][0]); #else // this will display the player at the collision shape position const dMatrix& visualMatrix = *((dMatrix*) matrix); #endif controller->m_setTransformOriginal (body, &visualMatrix[0][0], threadId); // now we will set the camera to follow the player dVector eyePoint (visualMatrix.TransformVector(controller->m_point)); // check if the player wants third person view static int prevCKeyDown = IsKeyDown ('C'); int isCkeyDwon = IsKeyDown ('C'); if (isCkeyDwon && !prevCKeyDown) { controller->m_isThirdView = !controller->m_isThirdView; } prevCKeyDown = isCkeyDwon; if (controller->m_isThirdView) { dVector dir (GetCameraDir ()); eyePoint -= dir.Scale (8.0f); } SetCameraEyePoint (eyePoint); // NewtonBodyGetMatrix (body, &matrix[0][0]); // cameraEyepoint = matrix.m_posit; // cameraEyepoint.m_y += 1.0f; }
NewtonCustomJoint::~NewtonCustomJoint() { //_ASSERTE (m_joint); //if the joint has user data it means the application is destroy the joint if (NewtonJointGetUserData (m_joint)) { // set the joint call to NULL to prevent infinite recursion NewtonJointSetDestructor (m_joint, NULL); // destroy this joint NewtonDestroyJoint(m_world, m_joint); } }
void CustomJoint::SubmitConstraints (const NewtonJoint* const me, dFloat timestep, int threadIndex) { // get the pointer to the joint class CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (me); // call the constraint call back joint->SubmitConstraints(timestep, threadIndex); // if there is a user define callback call it from here; if (joint->m_userConstrationCallback) { joint->m_userConstrationCallback ((const NewtonUserJoint*) joint, timestep, threadIndex); } }
void CustomJoint::Serialize (const NewtonJoint* const me, NewtonSerializeCallback callback, void* const userData) { CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (me); dCRCTYPE key = joint->GetSerializeKey(); callback (userData, &key, sizeof (key)); const SerializeMetaDataDictionary& dictionary = GetDictionary(); SerializeMetaDataDictionary::dTreeNode* const node = dictionary.Find(key); if (node) { SerializeMetaData* const meta = node->GetInfo(); meta->SerializeJoint(joint, callback, userData); } }
void NewtonCustomJoint::Destructor (const NewtonJoint* me) { NewtonCustomJoint* joint; // get the pointer to the joint class joint = (NewtonCustomJoint*) NewtonJointGetUserData (me); // set the joint call to NULL to prevent infinite recursion NewtonJointSetDestructor (me, NULL); NewtonJointSetUserData (me, NULL); // delete the joint class delete joint; }
dCustomJoint* dSkeletonBone::GetParentJoint() const { if (m_parent) { for (NewtonJoint* joint = NewtonBodyGetFirstJoint(m_body); joint; joint = NewtonBodyGetNextJoint(m_body, joint)) { dCustomJoint* const customJoint = (dCustomJoint*)NewtonJointGetUserData(joint); dAssert(customJoint); if (((customJoint->GetBody0() == m_body) && (customJoint->GetBody1() == m_parent->m_body)) || ((customJoint->GetBody1() == m_body) && (customJoint->GetBody0() == m_parent->m_body))) { return customJoint; } } dAssert(0); } return NULL; }
/** * @brief * Static Newton joint user callback function */ unsigned JointUniversal::JointUserCallback(const Newton::NewtonJoint *pNewtonJoint, Newton::NewtonHingeSliderUpdateDesc *pDesc) { // Get joint const JointUniversal *pJoint = static_cast<JointUniversal*>(NewtonJointGetUserData(pNewtonJoint)); if (!pJoint) return 0; // [TODO] Breakable // Get min/max angle limits const float fAngleMinLimit1 = pJoint->GetLowRange1(); const float fAngleMaxLimit1 = pJoint->GetHighRange1(); const float fAngleMinLimit2 = pJoint->GetLowRange2(); const float fAngleMaxLimit2 = pJoint->GetHighRange2(); // Check limits uint32 nReturn = 0; // Limit 1 float fAngle = NewtonUniversalGetJointAngle0(pNewtonJoint); if (fAngle > fAngleMaxLimit1) { // If the joint angle is large than the defined value, stop the universal pDesc[0].m_accel = NewtonUniversalCalculateStopAlpha0(pNewtonJoint, pDesc, fAngleMaxLimit1); nReturn |= 1; } else if (fAngle < fAngleMinLimit1) { // If the joint angle is smaller than the defined value, stop the universal pDesc[0].m_accel = NewtonUniversalCalculateStopAlpha0(pNewtonJoint, pDesc, fAngleMinLimit1); nReturn |= 1; } // Limit 2 fAngle = NewtonUniversalGetJointAngle1(pNewtonJoint); if (fAngle > fAngleMaxLimit2) { // If the joint angle is large than the defined value, stop the universal pDesc[1].m_accel = NewtonUniversalCalculateStopAlpha1(pNewtonJoint, &pDesc[1], fAngleMaxLimit2); nReturn |= 2; } else if (fAngle < fAngleMinLimit2) { // If the joint angle is smaller than the defined value, stop the universal pDesc[1].m_accel = NewtonUniversalCalculateStopAlpha1(pNewtonJoint, &pDesc[1], fAngleMinLimit2); nReturn |= 2; } // No action need it if the joint angle is with the limits return nReturn; }
CustomJoint::~CustomJoint() { //dAssert (m_joint); //if the joint has user data it means the application is destroying the joint // if there is a C destructor call it form here CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (m_joint); if (joint->m_userDestructor) { joint->m_userDestructor ((const NewtonUserJoint*) joint); } // if (NewtonJointGetUserData (m_joint)) { if (!m_autoDestroy) { // set the joint call to NULL to prevent infinite recursion NewtonJointSetUserData (m_joint, NULL); NewtonJointSetDestructor (m_joint, NULL); // destroy this joint NewtonDestroyJoint(m_world, m_joint); } }
void MSNewton::Slider::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); SliderData* cj_data = (SliderData*)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); const dVector& pos0 = matrix0.m_posit; dVector pos1(matrix1.m_posit + matrix1.m_right.Scale((pos0 - matrix1.m_posit) % matrix1.m_right)); // Calculate position, velocity, and acceleration dFloat last_pos = cj_data->cur_pos; dFloat last_vel = cj_data->cur_vel; cj_data->cur_pos = matrix1.UntransformVector(matrix0.m_posit).m_z; cj_data->cur_vel = (cj_data->cur_pos - last_pos) / timestep; cj_data->cur_accel = (cj_data->cur_vel - last_vel) / timestep; // Restrict movement on axis perpendicular to the pin direction. NewtonUserJointAddLinearRow(joint, &pos0[0], &pos1[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, &pos0[0], &pos1[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); // Add three angular rows to restrict rotation around all 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); NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_front, matrix1.m_front, matrix0.m_right), &matrix0.m_right[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);*/ // Get a point along the ping axis at some reasonable large distance from the pivot dVector q0(pos0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1(pos1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH)); // Add two constraints row perpendicular to the pin NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &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); NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &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); // Get a point along the ping axis at some reasonable large distance from the pivot dVector r0(pos0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector r1(pos1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); // Add one constraint row perpendicular to the pin NewtonUserJointAddLinearRow(joint, &r0[0], &r1[0], &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 limits and friction if (cj_data->limits_enabled == true && cj_data->cur_pos < cj_data->min - Joint::LINEAR_LIMIT_EPSILON) { const dVector& s0 = matrix0.m_posit; dVector s1(s0 + matrix1.m_right.Scale(cj_data->min - cj_data->cur_pos)); NewtonUserJointAddLinearRow(joint, &s0[0], &s1[0], &matrix1.m_right[0]); NewtonUserJointSetRowMinimumFriction(joint, 0.0f); 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); } else if (cj_data->limits_enabled == true && cj_data->cur_pos > cj_data->max + Joint::LINEAR_LIMIT_EPSILON) { const dVector& s0 = matrix0.m_posit; dVector s1(s0 + matrix1.m_right.Scale(cj_data->max - cj_data->cur_pos)); NewtonUserJointAddLinearRow(joint, &s0[0], &s1[0], &matrix1.m_right[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); 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); } else { dVector point(matrix1.UntransformVector(matrix0.m_posit)); point.m_z = 0.0f; point = matrix1.TransformVector(point); NewtonUserJointAddLinearRow(joint, &point[0], &matrix1.m_posit[0], &matrix1.m_right[0]); dFloat power = cj_data->friction * cj_data->controller; /*BodyData* cbody_data = (BodyData*)NewtonBodyGetUserData(joint_data->child); if (cbody_data->bstatic == false && cbody_data->mass >= MIN_MASS) power *= cbody_data->mass; else { BodyData* pbody_data = (BodyData*)NewtonBodyGetUserData(joint_data->child); if (pbody_data->bstatic == false && pbody_data->mass >= MIN_MASS) power *= pbody_data->mass; }*/ NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } }
void CustomJoint::GetInfo (const NewtonJoint* const me, NewtonJointRecord* info) { // get the pointer to the joint class CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (me); joint->GetInfo(info); }
void MSNewton::CurvySlider::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) { JointData* joint_data = (JointData*)NewtonJointGetUserData(joint); CurvySliderData* cj_data = (CurvySliderData*)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); dVector location = matrix2.UntransformVector(matrix0.m_posit); dVector point, vector, min_pt, max_pt; dFloat distance, min_len, max_len; if (!c_calc_curve_data_at_location(cj_data, location, point, vector, distance, min_pt, max_pt, min_len, max_len)) { cj_data->cur_data_set = false; return; } point = matrix2.TransformVector(point); vector = matrix2.RotateVector(vector); min_pt = matrix2.TransformVector(min_pt); max_pt = matrix2.TransformVector(max_pt); cj_data->cur_point = point; cj_data->cur_vector = vector; cj_data->cur_tangent = (1.0f - dAbs(vector.m_z) < EPSILON) ? Y_AXIS * vector : Z_AXIS * vector; cj_data->cur_data_set = true; dFloat last_pos = cj_data->cur_pos; dFloat last_vel = cj_data->cur_vel; if (cj_data->loop) { dFloat diff1 = distance - cj_data->last_dist; dFloat diff2 = diff1 + (diff1 > 0 ? -cj_data->curve_len : cj_data->curve_len); if (dAbs(diff1) < dAbs(diff2)) cj_data->cur_pos += diff1; else cj_data->cur_pos += diff2; } else cj_data->cur_pos = distance; cj_data->cur_vel = (cj_data->cur_pos - last_pos) / timestep; cj_data->cur_accel = (cj_data->cur_vel - last_vel) / timestep; cj_data->last_dist = distance; dMatrix matrix3; Util::matrix_from_pin_dir(point, vector, matrix3); const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix3.m_posit; dVector p00(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH)); dVector p11(p1 + matrix3.m_right.Scale(MIN_JOINT_PIN_LENGTH)); // Restrict movement on the pivot point along the normal and bi normal of the path. NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix3.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, &p0[0], &p1[0], &matrix3.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); // Align to curve if (cj_data->align) { NewtonUserJointAddLinearRow(joint, &p00[0], &p11[0], &matrix3.m_front[0]); if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); else NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); NewtonUserJointAddLinearRow(joint, &p00[0], &p11[0], &matrix3.m_up[0]); if (joint_data->ctype == CT_ROBUST) NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint)); else NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } // Add linear friction or limits dFloat min_posit = matrix3.UntransformVector(min_pt).m_z; dFloat max_posit = matrix3.UntransformVector(max_pt).m_z; dFloat cur_posit = matrix3.UntransformVector(p0).m_z; dFloat margin = EPSILON + 0.01f * dAbs(cj_data->cur_vel); if (cur_posit < min_posit - margin || (cur_posit < min_posit - Joint::LINEAR_LIMIT_EPSILON && dAbs(min_len) < EPSILON && cj_data->loop == false)) { NewtonUserJointAddLinearRow(joint, &p0[0], &min_pt[0], &matrix3.m_right[0]); NewtonUserJointSetRowMinimumFriction(joint, 0.0f); 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); } else if (cur_posit > max_posit + margin || (cur_posit > max_posit + Joint::LINEAR_LIMIT_EPSILON && dAbs(max_len - cj_data->curve_len) < EPSILON && cj_data->loop == false)) { NewtonUserJointAddLinearRow(joint, &p0[0], &max_pt[0], &matrix3.m_right[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); 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); } else { dVector point(matrix3.UntransformVector(matrix0.m_posit)); point.m_z = 0.0f; point = matrix3.TransformVector(point); NewtonUserJointAddLinearRow(joint, &point[0], &matrix3.m_posit[0], &matrix3.m_right[0]); dFloat power = cj_data->linear_friction * cj_data->controller; NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } // Add angular friction or limits if (cj_data->rotate) { if (cj_data->align) { NewtonUserJointAddAngularRow(joint, 0.0f, &matrix3.m_right[0]); dFloat power = cj_data->angular_friction * cj_data->controller; NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } else { dFloat cur_cone_angle_cos = matrix0.m_right % cj_data->last_dir; if (dAbs(cur_cone_angle_cos) < 0.99995f) { dVector lateral_dir = matrix0.m_right * cj_data->last_dir; Util::normalize_vector(lateral_dir); NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]); dFloat power = cj_data->angular_friction * cj_data->controller; NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->stiffness); } } } else if (cj_data->align) { NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_front, matrix3.m_front, matrix3.m_right), &matrix3.m_right[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); } else { // Get a point along the pin axis at some reasonable large distance from the pivot. dVector q0(p0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH)); dVector q1(p1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH)); // Get the ankle point. dVector r0(p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); dVector r1(p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH)); // Restrict rotation along all three orthonormal directions NewtonUserJointAddLinearRow(joint, &q0[0], &q1[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, &q0[0], &q1[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, &r0[0], &r1[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); } cj_data->last_dir = matrix0.m_right; }
void MSP::BallAndSocket::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) { MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint)); BallAndSocketData* cj_data = reinterpret_cast<BallAndSocketData*>(joint_data->m_cj_data); dFloat inv_timestep = 1.0f / timestep; // Calculate the position of the pivot point and the Jacobian direction vectors, in global space. dMatrix matrix0, matrix1; MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1); dFloat last_cone_angle = cj_data->m_cur_cone_angle; // Calculate current cone angle dFloat cur_cone_angle_cos = matrix0.m_right.DotProduct3(matrix1.m_right); cj_data->m_cur_cone_angle = dAcos(Util::clamp_float(cur_cone_angle_cos, -1.0f, 1.0f)); // Calculate current twist angle, omega, and acceleration. if (cur_cone_angle_cos < -0.999999f) { cj_data->m_cur_twist_omega = 0.0f; cj_data->m_cur_twist_alpha = 0.0f; } else { dFloat last_twist_angle = cj_data->m_twist_ai->get_angle(); dFloat last_twist_omega = cj_data->m_cur_twist_omega; dMatrix rot_matrix0; Util::rotate_matrix_to_dir(matrix0, matrix1.m_right, rot_matrix0); dFloat sin_angle; dFloat cos_angle; MSP::Joint::c_calculate_angle(matrix1.m_front, rot_matrix0.m_front, matrix1.m_right, sin_angle, cos_angle); cj_data->m_twist_ai->update(cos_angle, sin_angle); cj_data->m_cur_twist_omega = (cj_data->m_twist_ai->get_angle() - last_twist_angle) * inv_timestep; cj_data->m_cur_twist_alpha = (cj_data->m_cur_twist_omega - last_twist_omega) * inv_timestep; } // Get the current lateral and tangent dir dVector lateral_dir; dVector front_dir; if (dAbs(cur_cone_angle_cos) > 0.999999f) { lateral_dir = matrix1.m_front; front_dir = matrix1.m_up; } else { lateral_dir = matrix1.m_right.CrossProduct(matrix0.m_right); front_dir = matrix1.m_right.CrossProduct(lateral_dir); } // Restrict the movement on the pivot point along all tree orthonormal directions. NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); // Calculate friction dFloat power = cj_data->m_friction * dAbs(cj_data->m_controller); // Handle cone angle if (cj_data->m_cone_limits_enabled && cj_data->m_max_cone_angle < Joint::ANGULAR_LIMIT_EPSILON2) { // Handle in case joint being a hinge; max cone angle is near zero. NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_front), &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_up), &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_cone_limits_enabled && cj_data->m_cur_cone_angle > cj_data->m_max_cone_angle) { // Handle in case current cone angle is greater than max cone angle dFloat dangle = cj_data->m_cur_cone_angle - cj_data->m_max_cone_angle; NewtonUserJointAddAngularRow(joint, dangle, &lateral_dir[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else { // Handle in case limits are not necessary dFloat cur_cone_omega = (cj_data->m_cur_cone_angle - last_cone_angle) * inv_timestep; dFloat des_cone_accel = -cur_cone_omega * inv_timestep; NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]); NewtonUserJointSetRowAcceleration(joint, des_cone_accel); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } // Handle twist angle bool bcontinue = false; if (cj_data->m_twist_limits_enabled) { if (cj_data->m_min_twist_angle > cj_data->m_max_twist_angle) { // Handle in case min angle is greater than max NewtonUserJointAddAngularRow(joint, (cj_data->m_min_twist_angle + cj_data->m_max_twist_angle) * 0.5f - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_max_twist_angle - cj_data->m_min_twist_angle < Joint::ANGULAR_LIMIT_EPSILON2) { // Handle in case min angle is almost equal to max NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_twist_ai->get_angle() < cj_data->m_min_twist_angle) { // Handle in case current twist angle is less than min NewtonUserJointAddAngularRow(joint, cj_data->m_min_twist_angle - cj_data->m_twist_ai->get_angle() + Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]); NewtonUserJointSetRowMinimumFriction(joint, 0.0f); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_twist_ai->get_angle() > cj_data->m_max_twist_angle) { // Handle in case current twist angle is greater than max NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle() - Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else bcontinue = true; } else bcontinue = true; if (bcontinue) { // Handle in case limits are not necessary NewtonUserJointAddAngularRow(joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration(joint, -cj_data->m_cur_twist_omega * inv_timestep); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } }
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); } }