void CustomSlider::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
// three rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front), &matrix1.m_front[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dAssert (m_body0);
NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]);
if (m_body1) {
NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
m_lastRowWasUsed = false;
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
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 dCustomTireSpringDG::TireCenterPin(dFloat steptime)
{
// Wheel constraints simulation.
mCenterInTire = mTirePivotMatrix.m_posit;
//
mCenterInChassis = mChassisPivotMatrix.m_posit + mChassisPivotMatrix.m_up * (mCenterInTire - mChassisPivotMatrix.m_posit).DotProduct3(mChassisPivotMatrix.m_up);
//
NewtonUserJointAddLinearRow(m_joint, &mCenterInChassis[0], &mCenterInTire[0], &mChassisPivotMatrix.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &mCenterInChassis[0], &mCenterInTire[0], &mChassisPivotMatrix.m_right[0]);
}
void dCustomTireSpringDG::TireCenterBolt(dFloat steptime)
{
dVector pointInPinInTire;
dVector pointInPinInChassis;
//
pointInPinInTire = mCenterInChassis + (mChassisPivotMatrix.m_front * mAttachmentLength);
pointInPinInChassis = mCenterInTire + (mTirePivotMatrix.m_front * mAttachmentLength);
//
NewtonUserJointAddLinearRow(m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &mChassisPivotMatrix.m_right[0]);
NewtonUserJointAddLinearRow(m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &mChassisPivotMatrix.m_up[0]);
}
void CustomSlider::SubmitConstraintsFreeDof(dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
// if limit are enable ...
if (m_limitsOn) {
if (m_posit < m_minDist) {
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
m_lastRowWasUsed = true;
} else if (m_posit > m_maxDist) {
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
m_lastRowWasUsed = true;
} else {
/*
// uncomment this for a slider with friction
// take any point on body0 (origin)
const dVector& p0 = matrix0.m_posit;
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dVector omega1(0.0f);
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
NewtonBodyGetOmega(m_body1, &omega1[0]);
// this assumes the origin of the bodies the matrix pivot are the same
veloc1 += omega1 * (matrix1.m_posit - p0);
dFloat relAccel;
relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
m_lastRowWasUsed = false;
*/
}
}
}
void dCustomTireSpringDG::SuspenssionSpringLimits(dFloat steptime)
{
// Suspenssion limitas
if (mDistance < mMinSuspenssion) {
NewtonUserJointAddLinearRow(m_joint, &mCenterInChassis[0], &mCenterInChassis[0], &mChassisPivotMatrix.m_up[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0);
}
else
if (mDistance > mMaxSuspenssion) {
NewtonUserJointAddLinearRow(m_joint, &mCenterInChassis[0], &mCenterInChassis[0], &mChassisPivotMatrix.m_up[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0);
}
}
void CustomBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all three orthonormal directions
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
}
void PhyFixed::updateJoint(real timestep)
{
// If the joint is disabled, we don't do anything here
if (!m_shared->enabled) {
return;
}
// Updating the global matrices and the dof
updateJointInfo();
#ifdef WORLDSIM_USE_NEWTON
UNUSED_PARAM( timestep );
//--- Restrict the movement on the centre of the joint along all tree orthonormal direction
NewtonUserJointAddLinearRow( m_priv->joint, &m_shared->globalMatrixChild.w_pos[0], &m_shared->globalMatrixParent.w_pos[0], &m_shared->globalMatrixParent.x_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f );
NewtonUserJointAddLinearRow( m_priv->joint, &m_shared->globalMatrixChild.w_pos[0], &m_shared->globalMatrixParent.w_pos[0], &m_shared->globalMatrixParent.y_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f );
NewtonUserJointAddLinearRow( m_priv->joint, &m_shared->globalMatrixChild.w_pos[0], &m_shared->globalMatrixParent.w_pos[0], &m_shared->globalMatrixParent.z_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f );
//--- In order to constraint the rotation about X and Y axis of the joint
//--- we use LinearRow (that are stronger) getting a point far from objects along
//--- the Z axis. Doing this if the two object rotates about X or Y axes then
//--- the difference between qChild and qParent augments and then Newton Engine will apply
//--- a corresponding force (that applyied outside the centre of the object will become
//--- a torque) that will blocks any rotation about X and Y axis
real len = 5000.0;
wVector qChild( m_shared->globalMatrixChild.w_pos + m_shared->globalMatrixChild.z_ax.scale(len) );
wVector qParent( m_shared->globalMatrixParent.w_pos + m_shared->globalMatrixParent.z_ax.scale(len) );
NewtonUserJointAddLinearRow( m_priv->joint, &qChild[0], &qParent[0], &m_shared->globalMatrixParent.x_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0 );
NewtonUserJointAddLinearRow( m_priv->joint, &qChild[0], &qParent[0], &m_shared->globalMatrixParent.y_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0 );
NewtonUserJointAddAngularRow( m_priv->joint, 0.0f, &m_shared->globalMatrixParent.z_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0f );
//--- In order to do the same with third axis (Z), I need others point along different axis
/* qChild = wVector( m_shared->globalMatrixChild.w_pos + m_shared->globalMatrixChild.y_ax.scale(len) );
qParent = wVector( m_shared->globalMatrixParent.w_pos + m_shared->globalMatrixParent.y_ax.scale(len) );
NewtonUserJointAddLinearRow( m_priv->joint, &qChild[0], &qParent[0], &m_shared->globalMatrixParent.z_ax[0] );
NewtonUserJointSetRowStiffness( m_priv->joint, 1.0 );*/
//--- Retrive forces applied to the joint by the constraints
Shared* const d = m_shared.getModifiableShared();
d->forceOnJoint.x = NewtonUserJointGetRowForce (m_priv->joint, 0);
d->forceOnJoint.y = NewtonUserJointGetRowForce (m_priv->joint, 1);
d->forceOnJoint.z = NewtonUserJointGetRowForce (m_priv->joint, 2);
#endif
}
void CustomPathFollow::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
matrix0 = m_localMatrix0 * matrix0;
dMatrix matrix1 (EvalueCurve (matrix0.m_posit));
// Restrict the movement on the pivot point along all tree the normal and binormal of the path
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
// get a point along the ping axis at some reasonable large distance from the pivot
dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two more constraints rows to inforce the normal and binormal
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
// Julio: at this point the path follow will maintaing the pivot fixed to the path and will allow it to slide freelly
// it will also spin around the path tangent, if more control is needed then more constraion rows need to be submited
// for example friction along the oath tangent be added to make more realistic behavior
// or anothet point contration along the binormal to make no spin around the path tangent
/*
// example of ading friction
dVector veloc0;
dVector omega0;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetOmega(m_body0, &omega0[0]);
veloc0 += omega0 * (matrix0.m_posit - p0);
dFloat relAccel;
relAccel = - (veloc0 % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
void CustomConeLimitedBallAndSocket::SubmitConstrainst ()
{
dFloat coneCos;
dMatrix matrix0;
dMatrix matrix1;
// add the tree rows to keep the pivot in place
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// ///////////////////////////////////////////////////////////////////
//
// add a row to keep the child body inside the cone limit
//
// The child is inside the cone if the dCos of the angle between the pin and
coneCos = matrix0.m_front % matrix1.m_front;
if (coneCos < m_cosConeAngle) {
// the child body has violated the cone limit we need to stop it from keep moving
// for that we are going to pick a point along the the child body front vector
dVector p0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector p1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// get a vectors perpendicular to the plane of motion
dVector lateralDir (matrix0.m_front * matrix1.m_front);
// note this could fail if the angle between matrix0.m_front and matrix1.m_front is 90 degree
dVector unitLateralDir = lateralDir.Scale (1.0f / dSqrt (lateralDir % lateralDir));
// now we will add a constraint row along the lateral direction,
// this will add stability as it will prevent the child body from moving sideways
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &unitLateralDir[0]);
// calculate the unit vector tangent to the trajectory
dVector tangentDir (unitLateralDir * matrix0.m_front);
p1 = p0 + (p1 - p0).Scale (0.3f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &tangentDir[0]);
//we need to allow the body to mo in opposite direction to the penetration
//that can be done by setting the min friction to zero
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
void dCustomHinge::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dFloat sinAngle;
dFloat cosAngle;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// two rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
CalculateAngle (matrix1.m_up, matrix0.m_up, matrix1.m_front, sinAngle, cosAngle);
m_curJointAngle.Update(cosAngle, sinAngle);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_jointOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
m_lastRowWasUsed = false;
if (m_setAsSpringDamper) {
ApplySpringDamper (timestep, matrix0, matrix1);
} else {
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
}
void CustomHinge::SubmitConstrainst ()
{
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// Restrict the movemenet on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// get a point along the pin axis at some resonable large distance from the pivot
dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two contraints row perpendicular to the pin vector
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
// if limit are enable ...
if (m_limitsOn) {
// the joint angle can be determine by getting the angle between any two non parallel vectors
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = atan2f (sinAngle, cosAngle);
if (angle < m_minAngle) {
// get a point along the up vector and set a constraint
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximunFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle) {
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimunFriction (m_joint, 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 ApplySuspesionForce (
dFloat timestep,
const NewtonBody* thread, const dVector& threadPointLocal, const dMatrix& threadMatrix, const dVector& threadCOM, const dVector& threadVeloc, const dVector& threadOmega,
const NewtonBody* parent, const dVector& parentPointLocal, const dMatrix& parentMatrix, const dVector& parentCOM, const dVector& parentVeloc, const dVector& parentOmega)
{
dFloat dist;
dFloat speed;
dFloat forceMag;
// calculate separation and speed of hard points
dVector threadPoint (threadMatrix.TransformVector(threadPointLocal));
dVector parentPoint (parentMatrix.TransformVector(parentPointLocal));
dist = (parentPoint - threadPoint) % parentMatrix.m_up;
speed = ((parentVeloc + parentOmega * (parentPoint - parentCOM) -
threadVeloc - threadOmega * (threadPoint - threadCOM)) % parentMatrix.m_up);
if (dist > MAX_COMPRESION_DIST) {
NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
} else if (dist < MIN_EXPANSION_DIST) {
// submit a contact constraint to prevent the body
NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
// apply the spring force
forceMag = NewtonCalculateSpringDamperAcceleration (timestep, SPRING_CONST, dist, DAMPER_CONST, speed) * m_massScale;
dVector forceParent (parentMatrix.m_up.Scale (forceMag));
dVector torqueParent ((parentPoint - parentCOM) * forceParent);
NewtonBodyAddForce(m_body1, &forceParent[0]);
NewtonBodyAddTorque(m_body1, &torqueParent[0]);
dVector forceThread (forceParent.Scale (-1.0f));
dVector torqueThread ((threadPoint - threadCOM) * forceThread);
NewtonBodyAddForce(m_body0, &forceThread[0]);
NewtonBodyAddTorque(m_body0, &torqueThread[0]);
}
void CustomDGRayCastCar::ApplyTireFrictionVelocitySiding (Tire& tire, const dMatrix& chassisMatrix, const dVector& tireAxelVeloc, const dVector& tireAxelPosit, dFloat timestep, dFloat invTimestep)
{
dFloat invMag2;
dFloat frictionCircleMag;
dFloat lateralFrictionForceMag;
dFloat longitudinalFrictionForceMag;
// dFloat tireContactSpeed;
// dFloat tireRelativeSpeed;
// dFloat lateralForceMagnitud;
// calculate relative velocity at the tire center
dVector tireAxelRelativeVelocity (tireAxelVeloc - tire.m_hitBodyPointVelocity);
// now calculate relative velocity a velocity at contact point
dVector tireAngularVelocity ( tire.m_lateralPin.Scale (tire.m_angularVelocity));
dVector tireRadius (tire.m_contactPoint - tireAxelPosit);
dVector tireContactVelocity ( tireAngularVelocity * tireRadius);
dVector tireContactRelativeVelocity ( tireAxelRelativeVelocity + tireContactVelocity );
// Apply brake, need some little fix here.
// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
if ( dAbs( tire.m_breakForce ) > 1.0e-3f ) {
_ASSERTE (0);
// tire.m_isBraking = 1;
// tire.m_torque = 0.0f;
// tire.m_turnforce = tire.m_turnforce * 0.5f;
// tire.m_breakForce /= timestep;
// NewtonUserJointAddLinearRow ( m_joint, &tireAxelPosit[0], &tireAxelPosit[0], &chassisMatrix.m_front.m_x );
// NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce );
// NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce );
}
tire.m_breakForce = 0.0f;
//submit constrained for applying side forces.
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
lateralFrictionForceMag = frictionCircleMag;
longitudinalFrictionForceMag = tire.m_tireLoad;
invMag2 = frictionCircleMag / dSqrt ( lateralFrictionForceMag * lateralFrictionForceMag + longitudinalFrictionForceMag * longitudinalFrictionForceMag );
lateralFrictionForceMag *= invMag2;
longitudinalFrictionForceMag = invMag2;
NewtonUserJointAddLinearRow (m_joint, &tireAxelPosit[0], &tireAxelPosit[0], &tire.m_lateralPin[0]);
NewtonUserJointSetRowMaximumFriction (m_joint, lateralFrictionForceMag);
NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
// save the tire contact longitudinal velocity for integration after the solver
// tire.m_currentSlipVeloc = tireAxelRelativeVelocity % tire.m_longitudinalPin;
}
void SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
dVector p0 (matrix0.m_posit);
dVector p1 (matrix1.m_posit);
dVector dist (p1 - p0);
dFloat mag2 = dist % dist;
if (mag2 > 0.0f) {
dist = dist.Scale (1.0f / dSqrt (mag2));
p1 -= dist.Scale(m_distance);
}
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
}
void CustomSlider::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
// three rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculatePitchAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_front[0]);
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
if (m_body0) {
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
}
if (m_body1) {
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
// if limit are enable ...
m_hitLimitOnLastUpdate = false;
if (m_limitsOn) {
if (m_posit < m_minDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (m_posit > m_maxDist) {
// indicate that this row hit a limit
m_hitLimitOnLastUpdate = true;
// get a point along the up vector and set a constraint
const dVector& p0 = matrix0.m_posit;
dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else {
/*
// uncomment this for a slider with friction
// take any point on body0 (origin)
const dVector& p0 = matrix0.m_posit;
dVector veloc0;
dVector veloc1;
dVector omega1;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
NewtonBodyGetOmega(m_body1, &omega1[0]);
// this assumes the origin of the bodies the matrix pivot are the same
veloc1 += omega1 * (matrix1.m_posit - p0);
dFloat relAccel;
relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
}
}
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex)
{
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// get the pin fixed to the first body
const dVector& dir0 = matrix0.m_front;
// get the pin fixed to the second body
const dVector& dir1 = matrix1.m_up;
// construct an orthogonal coordinate system with these two vectors
dVector dir2 (dir0 * dir1);
dir2 = dir2.Scale (1.0f / dSqrt (dir2 % dir2));
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir0[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir1[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir2[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
dVector dir3 (dir2 * dir0);
dVector q0 (p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &dir0[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// check is the joint limit are enable
if (m_limit_0_On) {
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
if (angle < m_minAngle_0) {
dFloat relAngle;
relAngle = angle - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle_0) {
dFloat relAngle;
relAngle = angle - m_maxAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
dFloat relOmega;
dFloat relAccel;
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
relOmega = (omega0 - omega1) % matrix0.m_front;
relAccel = m_angularAccel_0 - m_angularDamp_0 * relOmega;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
angle = dAtan2 (sinAngle, cosAngle);
if (angle < m_minAngle_1) {
dFloat relAngle;
relAngle = angle - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle_1) {
dFloat relAngle;
relAngle = angle - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
dFloat relOmega;
dFloat relAccel;
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
relOmega = (omega0 - omega1) % matrix1.m_up;
relAccel = m_angularAccel_1 - m_angularDamp_1 * relOmega;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomCorkScrew::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// two rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// if limit are enable ...
if (m_limitsLinearOn) {
dFloat dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
if (dist < m_minLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (dist > m_maxLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
}
}
CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
dFloat angle = -m_curJointAngle.Update (cosAngle, sinAngle);
if (m_limitsAngularOn) {
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (angle < m_minAngularDist) {
dFloat relAngle = angle - m_minAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_minAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngularDist) {
dFloat relAngle = angle - m_maxAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_maxAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
if (m_angularmotorOn) {
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel - m_angularDamp * relOmega;
// if the motor capability is on, then set angular acceleration with zero angular correction
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the angular acceleration for this Jacobian to the desired acceleration
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{
// check if this is an impulsive time step
if (timestep > 0.0f) {
dMatrix matrix0;
dVector v(0.0f);
dVector w(0.0f);
dVector cg(0.0f);
dFloat invTimestep = 1.0f / timestep;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
NewtonBodyGetOmega (m_body0, &w[0]);
NewtonBodyGetVelocity (m_body0, &v[0]);
NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);
dVector p0 (matrix0.TransformVector (m_localHandle));
dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg));
dVector relPosit (m_targetPosit - p0);
dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
dVector relAccel (relVeloc.Scale (invTimestep * 0.3f));
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction);
if (m_pickMode) {
dQuaternion rotation;
NewtonBodyGetRotation (m_body0, &rotation.m_q0);
if (m_targetRot.DotProduct (rotation) < 0.0f) {
rotation.m_q0 *= -1.0f;
rotation.m_q1 *= -1.0f;
rotation.m_q2 *= -1.0f;
rotation.m_q3 *= -1.0f;
}
dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w);
dFloat mag = relOmega % relOmega;
if (mag > 1.0e-6f) {
dVector pin (relOmega.Scale (1.0f / mag));
dMatrix basis (dGrammSchmidt (pin));
dFloat relSpeed = dSqrt (relOmega % relOmega);
dFloat relAlpha = relSpeed * invTimestep;
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
dVector relAlpha (w.Scale (-invTimestep));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
} else {
// this is the single handle pick mode, add some angular friction
dVector relAlpha = w.Scale (-invTimestep);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f);
}
}
}
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::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 CustomPointToPoint::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit);
dVector dir(p1 - p0);
dFloat mag2 = dir % dir;
if (mag2 < 1.0e-3f) {
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
} else {
dir = dir.Scale(1.0f / dSqrt(mag2));
dMatrix matrix (dGrammSchmidt (dir));
dFloat x = dSqrt (mag2) - m_distance;
dVector com0;
dVector com1;
dVector veloc0;
dVector veloc1;
dMatrix body0Matrix;
dMatrix body1Matrix;
NewtonBody* const body0 = GetBody0();
NewtonBody* const body1 = GetBody1();
NewtonBodyGetCentreOfMass(body0, &com0[0]);
NewtonBodyGetMatrix(body0, &body0Matrix[0][0]);
NewtonBodyGetPointVelocity (body0, &p0[0], &veloc0[0]);
NewtonBodyGetCentreOfMass(body1, &com1[0]);
NewtonBodyGetMatrix(body1, &body1Matrix[0][0]);
NewtonBodyGetPointVelocity (body1, &p1[0], &veloc1[0]);
dFloat v((veloc0 - veloc1) % dir);
dFloat a = (x - v * timestep) / (timestep * timestep);
dVector r0 ((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front);
dVector r1 ((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front);
dFloat jacobian0[6];
dFloat jacobian1[6];
jacobian0[0] = matrix[0][0];
jacobian0[1] = matrix[0][1];
jacobian0[2] = matrix[0][2];
jacobian0[3] = r0[0];
jacobian0[4] = r0[1];
jacobian0[5] = r0[2];
jacobian1[0] = -matrix[0][0];
jacobian1[1] = -matrix[0][1];
jacobian1[2] = -matrix[0][2];
jacobian1[3] = -r1[0];
jacobian1[4] = -r1[1];
jacobian1[5] = -r1[2];
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix[1][0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix[2][0]);
NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1);
NewtonUserJointSetRowAcceleration(m_joint, a);
}
}
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
#if 0
dVector euler0;
dVector euler1;
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
for (int i = 0; i < 3; i ++) {
dFloat error = m_targetAngles[i] - euler[i];
if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) {
dFloat angularStep = dSign(error) * m_angulaSpeed * timestep;
if (angularStep > 0.0f) {
if (angularStep > error) {
angularStep = error * 0.5f;
}
} else {
if (angularStep < error) {
angularStep = error * 0.5f;
}
}
euler[i] = euler[i] + angularStep;
}
}
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]);
}
#else
matrix1 = m_targetRotation * matrix1;
dQuaternion localRotation(matrix1 * matrix0.Inverse());
if (localRotation.DotProduct(m_targetRotation) < 0.0f) {
localRotation.Scale(-1.0f);
}
dFloat angle = 2.0f * dAcos(localRotation.m_q0);
dFloat angleStep = m_angulaSpeed * timestep;
if (angleStep < angle) {
dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f));
axis = axis.Scale(1.0f / dSqrt(axis % axis));
// localRotation = dQuaternion(axis, angleStep);
}
dVector axis (matrix1.m_front * matrix1.m_front);
dVector axis1 (matrix1.m_front * matrix1.m_front);
//dFloat sinAngle;
//dFloat cosAngle;
//CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
//float xxxx = dAtan2(sinAngle, cosAngle);
//float xxxx1 = dAtan2(sinAngle, cosAngle);
dQuaternion quat(localRotation);
if (quat.m_q0 > dFloat(0.99995f)) {
// dAssert (0);
/*
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
*/
} else {
dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
}
#endif
}
void CustomSlidingContact::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dFloat sinAngle;
dFloat cosAngle;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front));
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
// construct an orthogonal coordinate system with these two vectors
dMatrix matrix1_1;
matrix1_1.m_up = matrix1.m_up;
matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
matrix1_1.m_right = matrix1_1.m_right.Scale(1.0f / dSqrt(matrix1_1.m_right % matrix1_1.m_right));
matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_front), &matrix1_1.m_front[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_right), &matrix1_1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
CalculateAngle(matrix1_1.m_front, matrix1.m_front, matrix1.m_up, sinAngle, cosAngle);
m_curJointAngle.Update(cosAngle, sinAngle);
dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
dAssert(m_body0);
NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]);
if (m_body1) {
NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
// if limit are enable ...
if (m_limitsLinearOn) {
if (m_posit < m_minLinearDist) {
// get a point along the up vector and set a constraint
dVector p (matrix1.m_posit + matrix1.m_front.Scale(m_minLinearDist));
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (m_posit > m_maxLinearDist) {
dVector p(matrix1.m_posit + matrix1.m_front.Scale(m_maxLinearDist));
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
if (m_limitsAngularOn) {
dFloat angle1 = m_curJointAngle.GetAngle();
if (angle1 < m_minAngularDist) {
dFloat relAngle = angle1 - m_minAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
else if (angle1 > m_maxAngularDist) {
dFloat relAngle = angle1 - m_maxAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
}
}
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
// construct an orthogonal coordinate system with these two vectors
dMatrix matrix1_1;
matrix1_1.m_up = matrix1.m_up;
matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right));
matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]);
dFloat sinAngle_0;
dFloat cosAngle_0;
CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0);
dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0);
dFloat sinAngle_1;
dFloat cosAngle_1;
CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1);
dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1);
dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dVector relOmega (omega0 - omega1);
m_jointOmega_0 = relOmega % matrix0.m_front;
m_jointOmega_1 = relOmega % matrix1.m_up;
// check is the joint limit are enable
if (m_limit_0_On) {
if (angle0 < m_minAngle_0) {
dFloat relAngle = angle0 - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle0 > m_maxAngle_0) {
dFloat relAngle = angle0 - m_maxAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
// calculate the desired acceleration
// dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
if (angle1 < m_minAngle_1) {
dFloat relAngle = angle1 - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle1 > m_maxAngle_1) {
dFloat relAngle = angle1 - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
// calculate the desired acceleration
dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// add the linear limits
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
// it is a limited linear dof, check if it pass the limits
dFloat dist = dp.DotProduct3(matrix1[i]);
if (dist > m_maxLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (dist < m_minLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
}
dVector euler0(0.0f);
dVector euler1(0.0f);
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));
bool limitViolation = false;
for (int i = 0; i < 3; i ++) {
if (euler[i] < m_minAngularLimits[i]) {
limitViolation = true;
euler[i] = m_minAngularLimits[i];
} else if (euler[i] > m_maxAngularLimits[i]) {
limitViolation = true;
euler[i] = m_maxAngularLimits[i];
}
}
if (limitViolation) {
//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
}
}
}
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
// get the simulation time
// dFloat invTimestep = 1.0f / timestep ;
// get the vehicle global matrix, and use it in several calculations
dMatrix bodyMatrix;
NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
dMatrix chassisMatrix (m_localFrame * bodyMatrix);
// get the chassis instantaneous linear and angular velocity in the local space of the chassis
dVector bodyForce;
dVector bodyOmega;
dVector bodyVelocity;
NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
NewtonBodyGetOmega (m_body0, &bodyOmega[0]);
//static int xxx;
//dTrace (("frame %d veloc(%f %f %f)\n", xxx, bodyVelocity[0], bodyVelocity[1], bodyVelocity[2]));
//xxx ++;
//if (xxx >= 210) {
//xxx *=1;
//bodyVelocity.m_x = 0;
//bodyVelocity.m_z = 10;
//NewtonBodySetVelocity (m_body0, &bodyVelocity[0]);
//}
// dVector normalForces (0.0f, 0.0f, 0.0f, 0.0f);
// all tire is on air check
m_vehicleOnAir = 0;
// int constraintIndex = 0;
for (int i = 0; i < m_tiresCount; i ++) {
// dTrace (("tire: %d ", i));
Tire& tire = m_tires[i];
tire.m_tireIsOnAir = 1;
// tire.m_tireIsConstrained = 0;
tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);
// calculate all suspension matrices in global space and tire collision
dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);
// calculate the tire collision
CalculateTireCollision (tire, suspensionMatrix, threadIndex);
// calculate the linear velocity of the tire at the ground contact
tire.m_tireAxelPositGlobal = chassisMatrix.TransformVector (tire.m_harpointInJointSpace - m_localFrame.m_up.Scale (tire.m_posit));
tire.m_tireAxelVelocGlobal = bodyVelocity + bodyOmega * (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);
tire.m_lateralPinGlobal = chassisMatrix.RotateVector (tire.m_localAxisInJointSpace);
tire.m_longitudinalPinGlobal = chassisMatrix.m_up * tire.m_lateralPinGlobal;
if (tire.m_posit < tire.m_suspensionLenght ) {
tire.m_tireIsOnAir = 0;
tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
if (tire.m_HitBody){
dMatrix matrix;
dVector com;
dVector omega;
NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
}
// calculate the relative velocity
dVector tireHubVeloc (tire.m_tireAxelVelocGlobal - tire.m_hitBodyPointVelocity);
dFloat suspensionSpeed = - (tireHubVeloc % chassisMatrix.m_up);
// now calculate the tire load at the contact point
// Tire suspension distance and hard limit.
dFloat distance = tire.m_suspensionLenght - tire.m_posit;
_ASSERTE (distance <= tire.m_suspensionLenght);
tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
if ( tire.m_tireLoad < 0.0f ) {
// since the tire is not a body with real mass it can only push the chassis.
tire.m_tireLoad = 0.0f;
}
//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
tire.m_tireLoad *= (m_mass * 0.5f);
// dTrace (("(load = %f) ", tire.m_tireLoad));
//tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);
// convert the tire load force magnitude to a torque and force.
// accumulate the force doe to the suspension spring and damper
tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);
// calculate relative velocity at the tire center
//dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
// axle linear speed
//axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;
dFloat axelLinearSpeed = tireHubVeloc % chassisMatrix.m_front;
// calculate tire rotation velocity at the tire radio
//dVector tireAngularVelocity (tire.m_lateralPinGlobal.Scale (tire.m_angularVelocity));
//dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPositGlobal);
//dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);
// calculate slip ratio and max longitudinal force
//dFloat tireRotationSpeed = -(tireRotationalVelocityAtContact % tire.m_longitudinalPinGlobal);
//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
//dTrace (("(slipRatio = %f) ", slipRatioCoef));
// calculate the formal longitudinal force the tire apply to the chassis
//dFloat longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
dFloat longitudinalForceMag = CalculateLongitudinalForce (i, axelLinearSpeed, tire.m_tireLoad * tire.m_groundFriction);
// dTrace (("(longForce = %f) ", longitudinalForceMag));
#if 0
// now calculate relative velocity a velocity at contact point
//dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact);
//dVector tireContactAbsoluteVelocity (tireHubVeloc + tireRotationalVelocityAtContact);
// calculate the side slip as the angle between the tire lateral speed and longitudinal speed
//dFloat lateralSpeed = tireContactRelativeVelocity % tire.m_lateralPin;
dFloat lateralSpeed = tireHubVeloc % tire.m_lateralPinGlobal;
dFloat sideSlipCoef = dAtan2 (dAbs (lateralSpeed), dAbs (axelLinearSpeed));
dFloat lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;
// Apply brake, need some little fix here.
// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
if ( tire.m_breakForce > 1.0e-3f ) {
_ASSERTE (0);
/*
// row constrained force is save for later determine the dynamic state of this tire
tire.m_isBrakingForceIndex = constraintIndex;
constraintIndex ++;
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
if (tire.m_breakForce > frictionCircleMag) {
tire.m_breakForce = frictionCircleMag;
}
//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x );
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);
// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
}
*/
}
//project the longitudinal and lateral forces over the circle of friction for this tire;
dFloat frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
dFloat tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
dFloat invMag2;
invMag2 = frictionCircleMag / dSqrt (tireForceMag);
longitudinalForceMag *= invMag2;
lateralFrictionForceMag *= invMag2;
}
// submit this constraint for calculation of side slip forces
lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
tire.m_lateralForceIndex = constraintIndex;
constraintIndex ++;
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPositGlobal[0], &tire.m_tireAxelPositGlobal[0], &tire.m_lateralPinGlobal[0]);
NewtonUserJointSetRowMaximumFriction (m_joint, lateralFrictionForceMag);
NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
#endif
// accumulate the longitudinal force
dVector tireForce (tire.m_longitudinalPinGlobal.Scale (longitudinalForceMag));
tire.m_tireForceAcc += tireForce;
// now we apply the combined tire force generated by this tire, to the car chassis
dVector r (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);
// add the toque the tire asserts on the car body (principle of action reaction)
dVector torque (r * tire.m_tireForceAcc - tire.m_lateralPinGlobal.Scale (tire.m_torque));
NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
NewtonBodyAddTorque( m_body0, &torque[0] );
/*
// calculate the net torque on the tire
dFloat tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPinGlobal);
if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
// the tire reaction force cannot be larger than the applied engine torque
// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
tire.m_tireIsConstrained = 1;
tireTorqueMag = tire.m_torque;
}
tire.m_torque -= tireTorqueMag;
*/
// normalForces += tire.m_tireForceAcc;
} else {
// there is a next torque on the tire
tire.m_torque -= tire.m_angularVelocity * tire.m_Ixx * DG_TIRE_VISCUOS_DAMP;
tire.m_angularVelocity += tire.m_torque * tire.m_IxxInv * timestep;
if (m_tires[i].m_breakForce > dFloat (0.1f)) {
tire.m_angularVelocity = 0.0f;
}
}
// dTrace (("(tireTorque = %f) ", tire.m_torque));
// spin the tire by the angular velocity
tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);
// reset the tire torque
tire.m_torque = 0.0f;
tire.m_breakForce = 0.0f;
// dTrace (("\n"));
}
// add a row to simulate the engine rolling resistance
// float bodyWeight = dAbs (normalForces % chassisMatrix.m_up) * m_rollingResistance;
// if (bodyWeight > (1.0e-3f) * m_mass) {
// NewtonUserJointAddLinearRow (m_joint, &chassisMatrix.m_posit[0], &chassisMatrix.m_posit[0], &chassisMatrix.m_front[0]);
// NewtonUserJointSetRowMaximumFriction( m_joint, bodyWeight);
// NewtonUserJointSetRowMinimumFriction( m_joint, -bodyWeight);
// }
}
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
int constraintIndex;
dFloat invTimestep;
dMatrix bodyMatrix;
// get the simulation time
invTimestep = 1.0f / timestep ;
// get the vehicle global matrix, and use it in several calculations
NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
dMatrix chassisMatrix (m_localFrame * bodyMatrix);
// get the chassis instantaneous linear and angular velocity in the local space of the chassis
dVector bodyOmega;
dVector bodyVelocity;
NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
NewtonBodyGetOmega (m_body0, &bodyOmega[0]);
// all tire is on air check
m_vehicleOnAir = 0;
constraintIndex = 0;
for ( int i = 0; i < m_tiresCount; i ++ ) {
Tire& tire = m_tires[i];
tire.m_tireIsOnAir = 1;
tire.m_tireIsConstrained = 0;
tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);
// calculate all suspension matrices in global space and tire collision
dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);
// calculate the tire collision
CalculateTireCollision (tire, suspensionMatrix, threadIndex);
// calculate the linear velocity of the tire at the ground contact
tire.m_tireAxelPosit = chassisMatrix.TransformVector( tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit));
tire.m_tireAxelVeloc = bodyVelocity + bodyOmega * (tire.m_tireAxelPosit - chassisMatrix.m_posit);
tire.m_lateralPin = ( chassisMatrix.RotateVector ( tire.m_localAxis ) );
tire.m_longitudinalPin = ( chassisMatrix.m_up * tire.m_lateralPin );
if (tire.m_posit < tire.m_suspensionLenght ) {
dFloat distance;
dFloat sideSlipCoef;
dFloat slipRatioCoef;
dFloat tireForceMag;
dFloat tireTorqueMag;
dFloat suspensionSpeed;
dFloat axelLinealSpeed;
dFloat tireRotationSpeed;
dFloat frictionCircleMag;
dFloat longitudinalForceMag;
dFloat lateralFrictionForceMag;
tire.m_tireIsOnAir = 0;
tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
if (tire.m_HitBody){
dMatrix matrix;
dVector com;
dVector omega;
NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
}
// calculate the relative velocity
dVector relVeloc (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
suspensionSpeed = - (relVeloc % chassisMatrix.m_up);
// now calculate the tire load at the contact point
// Tire suspension distance and hard limit.
distance = tire.m_suspensionLenght - tire.m_posit;
_ASSERTE (distance <= tire.m_suspensionLenght);
tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
if ( tire.m_tireLoad < 0.0f ) {
// since the tire is not a body with real mass it can only push the chassis.
tire.m_tireLoad = 0.0f;
}
//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
tire.m_tireLoad *= (m_mass * 0.5f);
tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);
// convert the tire load force magnitude to a torque and force.
// accumulate the force doe to the suspension spring and damper
tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);
// calculate relative velocity at the tire center
dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
// axle linear speed
axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;
// calculate tire rotation velocity at the tire radio
dVector tireAngularVelocity (tire.m_lateralPin.Scale (tire.m_angularVelocity));
dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);
longitudinalForceMag = 0.0f;
// if (!tire.m_tireIsConstrained) {
// calculate slip ratio and max longitudinal force
tireRotationSpeed = tireRotationalVelocityAtContact % tire.m_longitudinalPin;
slipRatioCoef = (dAbs (axelLinealSpeed) > 1.e-3f) ? ((-tireRotationSpeed - axelLinealSpeed) / dAbs (axelLinealSpeed)) : 0.0f;
// calculate the formal longitudinal force the tire apply to the chassis
longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
// }
// now calculate relative velocity a velocity at contact point
dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact);
// calculate the sideslip as the angle between the tire lateral speed and longitudila speed
sideSlipCoef = dAtan2 (dAbs (tireContactRelativeVelocity % tire.m_lateralPin), dAbs (axelLinealSpeed));
lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;
// Apply brake, need some little fix here.
// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
if ( tire.m_breakForce > 1.0e-3f ) {
// row constrained force is save for later determine the dynamic state of this tire
tire.m_isBrakingForceIndex = constraintIndex;
constraintIndex ++;
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
if (tire.m_breakForce > frictionCircleMag) {
tire.m_breakForce = frictionCircleMag;
}
//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x );
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);
// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
}
}
//project the longitudinal and lateral forces over the circle of friction for this tire;
frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
dFloat invMag2;
invMag2 = frictionCircleMag / dSqrt (tireForceMag);
longitudinalForceMag *= invMag2;
lateralFrictionForceMag *= invMag2;
}
// submit this constraint for calculation of side slip forces
lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
tire.m_lateralForceIndex = constraintIndex;
constraintIndex ++;
NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_lateralPin[0]);
NewtonUserJointSetRowMaximumFriction (m_joint, lateralFrictionForceMag);
NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
// accumulate the longitudinal force
dVector tireForce (tire.m_longitudinalPin.Scale (longitudinalForceMag));
tire.m_tireForceAcc += tireForce;
// now we apply the combined tire force generated by this tire, to the car chassis
dVector torque ((tire.m_tireAxelPosit - chassisMatrix.m_posit) * tire.m_tireForceAcc);
NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
NewtonBodyAddTorque( m_body0, &torque[0] );
// calculate the net torque on the tire
tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPin);
if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
// the tire reaction force can no be larger than the applied engine torque
// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
tire.m_tireIsConstrained = 1;
tireTorqueMag = tire.m_torque;
}
tire.m_torque -= tireTorqueMag;
}
}
}
void CustomLimitBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
matrix1 = m_rotationOffset * matrix1;
// handle special case of the joint being a hinge
if (m_coneAngleCos > 0.9999f) {
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
} else {
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngle = coneDir0 % coneDir1;
if (cosAngle <= m_coneAngleCos) {
dVector lateralDir(coneDir0 * coneDir1);
dFloat mag2 = lateralDir % lateralDir;
dAssert(mag2 > 1.0e-4f);
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
dQuaternion rot(m_coneAngleHalfCos, lateralDir.m_x * m_coneAngleHalfSin, lateralDir.m_y * m_coneAngleHalfSin, lateralDir.m_z * m_coneAngleHalfSin);
dVector frontDir(rot.UnrotateVector(coneDir1));
dVector upDir(lateralDir * frontDir);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(coneDir0, frontDir, lateralDir), &lateralDir[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
//handle twist angle
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
}
}
