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);
}
}