dCustomJoint::dAngularIntegration dCustomJoint::dAngularIntegration::operator- (const dAngularIntegration& angle) const
{
dFloat sin_da = angle.m_sinJointAngle * m_cosJointAngle - angle.m_cosJointAngle * m_sinJointAngle;
dFloat cos_da = angle.m_cosJointAngle * m_cosJointAngle + angle.m_sinJointAngle * m_sinJointAngle;
dFloat angle_da = dAtan2(sin_da, cos_da);
return dAngularIntegration(angle_da);
}
void DemoCamera::SetMatrix (DemoEntityManager& scene, const dQuaternion& rotation, const dVector& position)
{
dMatrix matrix (rotation, position);
m_cameraPitch = dAsin (matrix.m_front.m_y);
m_cameraYaw = dAtan2 (-matrix.m_front.m_z, matrix.m_front.m_x);
DemoEntity::SetMatrix (scene, rotation, position);
}
dFloat AngularIntegration::update(dFloat new_angle_cos, dFloat new_angle_sin) {
dFloat sin_da = new_angle_sin * m_cos_angle - new_angle_cos * m_sin_angle;
dFloat cos_da = new_angle_cos * m_cos_angle + new_angle_sin * m_sin_angle;
m_angle += dAtan2(sin_da, cos_da);
m_cos_angle = new_angle_cos;
m_sin_angle = new_angle_sin;
return m_angle;
}
dReal
dxJointHinge2::measureAngle() const
{
dVector3 a1, a2;
dMultiply0_331( a1, node[1].body->posr.R, axis2 );
dMultiply1_331( a2, node[0].body->posr.R, a1 );
dReal x = dCalcVectorDot3( v1, a2 );
dReal y = dCalcVectorDot3( v2, a2 );
return -dAtan2( y, x );
}
dFloat dCustomJoint::dAngularIntegration::Update(dFloat newAngleCos, dFloat newAngleSin)
{
dFloat sin_da = newAngleSin * m_cosJointAngle - newAngleCos * m_sinJointAngle;
dFloat cos_da = newAngleCos * m_cosJointAngle + newAngleSin * m_sinJointAngle;
m_angle += dAtan2(sin_da, cos_da);
m_cosJointAngle = newAngleCos;
m_sinJointAngle = newAngleSin;
return m_angle;
}
void CustomCorkScrew::GetInfo (NewtonJointRecord* const info) const
{
strcpy (info->m_descriptionType, "corkScrew");
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
if (m_limitsLinearOn) {
dFloat dist;
dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
info->m_minLinearDof[0] = m_minLinearDist - dist;
info->m_maxLinearDof[0] = m_maxLinearDist - dist;
} else {
info->m_minLinearDof[0] = -FLT_MAX ;
info->m_maxLinearDof[0] = FLT_MAX ;
}
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
// info->m_minAngularDof[0] = -FLT_MAX;
// info->m_maxAngularDof[0] = FLT_MAX;
if (m_limitsAngularOn) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[0] = (m_minAngularDist - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngularDist - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -FLT_MAX ;
info->m_maxAngularDof[0] = FLT_MAX;
}
info->m_minAngularDof[1] = 0.0f;
info->m_maxAngularDof[1] = 0.0f;
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
void InitCamera (const dVector& eyePoint, const dVector& dir)
{
gCameraEyepoint = eyePoint;
gCurrCameraDir = dir.Scale (1.0f / sqrt (dir % dir));
gRollAngle = dAsin (gCurrCameraDir.m_y);
gPrevRollAngle = gYawAngle;
gYawAngle = dAtan2 (-gCurrCameraDir.m_z, gCurrCameraDir.m_x);
gPrevYawAngle = gYawAngle;
}
void CustomVehicleControllerComponentSteering::Update (dFloat timestep)
{
dFloat angle = m_maxAngle * m_param;
if ((m_akermanWheelBaseWidth == 0.0f) || (dAbs (angle) < (2.0f * 3.141592f / 180.0f))) {
for (dList<CustomVehicleControllerBodyStateTire*>::dListNode* node = m_steeringTires.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire& tire = *node->GetInfo();
tire.m_steeringAngle = angle;
}
} else {
dAssert (dAbs (angle) >= (2.0f * 3.141592f / 180.0f));
dFloat posit = m_akermanAxelSeparation / dTan (dAbs (angle));
dFloat sign = dSign (angle);
dFloat leftAngle = sign * dAtan2 (m_akermanAxelSeparation, posit + m_akermanWheelBaseWidth);
dFloat righAngle = sign * dAtan2 (m_akermanAxelSeparation, posit - m_akermanWheelBaseWidth);
for (dList<CustomVehicleControllerBodyStateTire*>::dListNode* node = m_steeringTires.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire& tire = *node->GetInfo();
tire.m_steeringAngle = (sign * tire.m_locationSign > 0.0f) ? leftAngle: righAngle;
}
}
}
void
dxJointAMotor::computeEulerAngles( dVector3 ax[3] )
{
// assumptions:
// global axes already calculated --> ax
// axis[0] is relative to body 1 --> global ax[0]
// axis[2] is relative to body 2 --> global ax[2]
// ax[1] = ax[2] x ax[0]
// original ax[0] and ax[2] are perpendicular
// reference1 is perpendicular to ax[0] (in body 1 frame)
// reference2 is perpendicular to ax[2] (in body 2 frame)
// all ax[] and reference vectors are unit length
// calculate references in global frame
dVector3 ref1, ref2;
dMultiply0_331( ref1, node[0].body->posr.R, reference1 );
if ( node[1].body )
{
dMultiply0_331( ref2, node[1].body->posr.R, reference2 );
}
else
{
ref2[0] = reference2[0];
ref2[1] = reference2[1];
ref2[2] = reference2[2];
}
// get q perpendicular to both ax[0] and ref1, get first euler angle
dVector3 q;
dCalcVectorCross3( q, ax[0], ref1 );
angle[0] = -dAtan2( dCalcVectorDot3( ax[2], q ), dCalcVectorDot3( ax[2], ref1 ) );
// get q perpendicular to both ax[0] and ax[1], get second euler angle
dCalcVectorCross3( q, ax[0], ax[1] );
angle[1] = -dAtan2( dCalcVectorDot3( ax[2], ax[0] ), dCalcVectorDot3( ax[2], q ) );
// get q perpendicular to both ax[1] and ax[2], get third euler angle
dCalcVectorCross3( q, ax[1], ax[2] );
angle[2] = -dAtan2( dCalcVectorDot3( ref2, ax[1] ), dCalcVectorDot3( ref2, q ) );
}
dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
{
// the angle between the two bodies is extracted from the quaternion that
// represents the relative rotation between them. recall that a quaternion
// q is:
// [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
// where s is a scalar and v is a 3-vector. u is a unit length axis and
// theta is a rotation along that axis. we can get theta/2 by:
// theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
// but we can't get sin(theta/2) directly, only its absolute value, i.e.:
// |v| = |sin(theta/2)| * |u|
// = |sin(theta/2)|
// using this value will have a strange effect. recall that there are two
// quaternion representations of a given rotation, q and -q. typically as
// a body rotates along the axis it will go through a complete cycle using
// one representation and then the next cycle will use the other
// representation. this corresponds to u pointing in the direction of the
// hinge axis and then in the opposite direction. the result is that theta
// will appear to go "backwards" every other cycle. here is a fix: if u
// points "away" from the direction of the hinge (motor) axis (i.e. more
// than 90 degrees) then use -q instead of q. this represents the same
// rotation, but results in the cos(theta/2) value being sign inverted.
// extract the angle from the quaternion. cost2 = cos(theta/2),
// sint2 = |sin(theta/2)|
dReal cost2 = qrel[0];
dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direction of axis
( 2 * dAtan2( sint2, -cost2 ) ); // if u points in opposite direction
// the angle we get will be between 0..2*pi, but we want to return angles
// between -pi..pi
if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );
// the angle we've just extracted has the wrong sign
theta = -theta;
return theta;
}
void CustomHinge::GetInfo (NewtonJointRecord* info) const
{
strcpy (info->m_descriptionType, "hinge");
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
info->m_minLinearDof[0] = 0.0f;
info->m_maxLinearDof[0] = 0.0f;
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (m_limitsOn) {
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);
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[0] = (m_minAngle - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngle - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -FLT_MAX ;
info->m_maxAngularDof[0] = FLT_MAX ;
}
info->m_minAngularDof[1] = 0.0f;
info->m_maxAngularDof[1] = 0.0f;
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}
dReal
dxJointHinge2::measureAngle1() const
{
// bring axis 2 into first body's reference frame
dVector3 p, q;
if (node[1].body)
dMultiply0_331( p, node[1].body->posr.R, axis2 );
else
dCopyVector3(p, axis2);
if (node[0].body)
dMultiply1_331( q, node[0].body->posr.R, p );
else
dCopyVector3(q, p);
dReal x = dCalcVectorDot3( v1, q );
dReal y = dCalcVectorDot3( v2, q );
return -dAtan2( y, x );
}
dReal
dxJointHinge2::measureAngle2() const
{
// bring axis 1 into second body's reference frame
dVector3 p, q;
if (node[0].body)
dMultiply0_331( p, node[0].body->posr.R, axis1 );
else
dCopyVector3r4(p, axis1);
if (node[1].body)
dMultiply1_331( q, node[1].body->posr.R, p );
else
dCopyVector3r4(q, p);
dReal x = dCalcVectorDot3( w1, q );
dReal y = dCalcVectorDot3( w2, q );
return -dAtan2( y, x );
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const
{
dQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dQuaternion dq (q0.Inverse() * q1);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dFloat dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dFloat dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dFloat dirMag = dirMag2 * dirMagInv;
dFloat omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &QB, dFloat dt) const
{
dFloat dirMag;
dFloat dirMag2;
dFloat omegaMag;
dFloat dirMagInv;
dQuaternion dq (Inverse() * QB);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dirMag = dirMag2 * dirMagInv;
omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) / dt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
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 dCustomBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
SubmitLinearRows(0x07, matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
dMatrix coneRotation(dGetIdentityMatrix());
dVector lateralDir(matrix0.m_up);
if (cosAngleCos < 0.9999f) {
lateralDir = coneDir1.CrossProduct(coneDir0);
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale (-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
}
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
// do twist angle calculations
dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
if (m_options.m_option0) {
if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else {
SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
}
}
} else if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else if (m_twistFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
}
// do twist cone angle calculations
if (m_options.m_option2) {
if ((m_maxConeAngle == 0.0f)) {
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else {
dFloat jointOmega = relOmega.DotProduct3(lateralDir);
dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
dFloat coneAngle = currentAngle + jointOmega * timestep;
if (coneAngle >= m_maxConeAngle) {
//dQuaternion rot(lateralDir, coneAngle);
//dVector frontDir(rot.RotateVector(coneDir1));
//dVector upDir(lateralDir.CrossProduct(frontDir));
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (m_coneFriction != 0) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
}
} else if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else if (m_coneFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
void 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 CustomCorkScrew::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// two rows to restrict rotation around around the parent coordinate system
dFloat sinAngle;
dFloat cosAngle;
CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);
CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);
// if limit are enable ...
if (m_limitsLinearOn) {
dFloat dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
if (dist < m_minLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (dist > m_maxLinearDist) {
// get a point along the up vector and set a constraint
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
}
}
CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
dFloat angle = -m_curJointAngle.Update (cosAngle, sinAngle);
if (m_limitsAngularOn) {
// the joint angle can be determine by getting the angle between any two non parallel vectors
if (angle < m_minAngularDist) {
dFloat relAngle = angle - m_minAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_minAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngularDist) {
dFloat relAngle = angle - m_maxAngularDist;
// the angle was clipped save the new clip limit
//m_curJointAngle.m_angle = m_maxAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
if (m_angularmotorOn) {
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel - m_angularDamp * relOmega;
// if the motor capability is on, then set angular acceleration with zero angular correction
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the angular acceleration for this Jacobian to the desired acceleration
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex)
{
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
// get the pin fixed to the first body
const dVector& dir0 = matrix0.m_front;
// get the pin fixed to the second body
const dVector& dir1 = matrix1.m_up;
// construct an orthogonal coordinate system with these two vectors
dVector dir2 (dir0 * dir1);
dir2 = dir2.Scale (1.0f / dSqrt (dir2 % dir2));
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir0[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir1[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir2[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
dVector dir3 (dir2 * dir0);
dVector q0 (p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &dir0[0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// check is the joint limit are enable
if (m_limit_0_On) {
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
if (angle < m_minAngle_0) {
dFloat relAngle;
relAngle = angle - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle_0) {
dFloat relAngle;
relAngle = angle - m_maxAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
dFloat relOmega;
dFloat relAccel;
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
relOmega = (omega0 - omega1) % matrix0.m_front;
relAccel = m_angularAccel_0 - m_angularDamp_0 * relOmega;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
angle = dAtan2 (sinAngle, cosAngle);
if (angle < m_minAngle_1) {
dFloat relAngle;
relAngle = angle - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle > m_maxAngle_1) {
dFloat relAngle;
relAngle = angle - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
dFloat relOmega;
dFloat relAccel;
dVector omega0 (0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f);
// get relative angular velocity
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
relOmega = (omega0 - omega1) % matrix1.m_up;
relAccel = m_angularAccel_1 - m_angularDamp_1 * relOmega;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
dFloat CustomJoint::CalculateAngle (const dVector& dir, const dVector& cosDir, const dVector& sinDir, dFloat& sinAngle, dFloat& cosAngle) const
{
cosAngle = dir % cosDir;
sinAngle = (dir * cosDir) % sinDir;
return dAtan2(sinAngle, cosAngle);
}
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]);
// 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));
dVector dir3(dir1 * dir2);
dFloat sinAngle = ((dir3 * dir0) % dir2);
dFloat cosAngle = dir3 % dir0;
NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &dir2[0]);
dFloat sinAngle_0;
dFloat cosAngle_0;
CalculatePitchAngle(matrix0, matrix1, sinAngle_0, cosAngle_0);
dFloat angle0 = -m_curJointAngle_0.Update(cosAngle_0, sinAngle_0);
dFloat sinAngle_1;
dFloat cosAngle_1;
CalculateYawAngle(matrix0, matrix1, 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);
}
}
//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const
void dMatrix::GetEulerAngles (dVector & euler0, dVector & euler1, dEulerAngleOrder order) const
{
int a0 = (order >> 8) & 3;
int a1 = (order >> 4) & 3;
int a2 = (order >> 0) & 3;
const dMatrix & matrix = *this;
// Assuming the angles are in radians.
if (matrix[a0][a2] > 0.99995f)
{
dFloat picth0 = 0.0f;
dFloat yaw0 = -3.141592f * 0.5f;
dFloat roll0 = - dAtan2 (matrix[a2][a1], matrix[a1][a1]);
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth0;
euler1[a1] = yaw0;
euler1[a2] = roll0;
}
else
if (matrix[a0][a2] < -0.99995f)
{
dFloat picth0 = 0.0f;
dFloat yaw0 = 3.141592f * 0.5f;
dFloat roll0 = dAtan2 (matrix[a2][a1], matrix[a1][a1]);
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth0;
euler1[a1] = yaw0;
euler1[a2] = roll0;
}
else
{
//euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
//euler[a1] = -dAsin ( matrix[a0][a2]);
//euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);
dFloat yaw0 = -dAsin ( matrix[a0][a2]);
dFloat yaw1 = 3.141592f - yaw0;
dFloat sign0 = dSign (dCos (yaw0));
dFloat sign1 = dSign (dCos (yaw1));
dFloat picth0 = dAtan2 (matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
dFloat picth1 = dAtan2 (matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);
dFloat roll0 = dAtan2 (matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
dFloat roll1 = dAtan2 (matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);
if (yaw1 > 3.141592f)
yaw1 -= 2.0f * 3.141592f;
euler0[a0] = picth0;
euler0[a1] = yaw0;
euler0[a2] = roll0;
euler1[a0] = picth1;
euler1[a1] = yaw1;
euler1[a2] = roll1;
}
euler0[3] = dFloat (0.0f);
euler1[3] = dFloat (0.0f);
#ifdef _DEBUG
if (order == m_pitchYawRoll)
{
dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix (euler0[1]) * dRollMatrix (euler0[2]));
dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix (euler1[1]) * dRollMatrix (euler1[2]));
for (int i = 0; i < 3; i ++)
{
for (int j = 0; j < 3; j ++)
{
dFloat error = dAbs (m0[i][j] - matrix[i][j]);
dAssert (error < 5.0e-2f);
error = dAbs (m1[i][j] - matrix[i][j]);
dAssert (error < 5.0e-2f);
}
}
}
#endif
}
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 cullPoints (int n, dReal p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
dReal a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = REAL(0.5)*(p[0] + p[2]);
cy = REAL(0.5)*(p[1] + p[3]);
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
a += q;
cx += q*(p[i*2]+p[i*2+2]);
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
a = dRecip(REAL(3.0)*(a+q));
cx = a*(cx + q*(p[n*2-2]+p[0]));
cy = a*(cy + q*(p[n*2-1]+p[1]));
}
// compute the angle of each point w.r.t. the centroid
dReal A[8];
for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
// search for points that have angles closest to A[i0] + i*(2*pi/m).
int avail[8];
for (i=0; i<n; i++) avail[i] = 1;
avail[i0] = 0;
iret[0] = i0;
iret++;
for (j=1; j<m; j++) {
a = (dReal)(dReal(j)*(2*M_PI/m) + A[i0]);
if (a > M_PI) a -= (dReal)(2*M_PI);
dReal maxdiff=1e9,diff;
#ifndef dNODEBUG
*iret = i0; // iret is not allowed to keep this value
#endif
for (i=0; i<n; i++) {
if (avail[i]) {
diff = dFabs (A[i]-a);
if (diff > M_PI) diff = (dReal) (2*M_PI - diff);
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
#ifndef dNODEBUG
dIASSERT (*iret != i0); // ensure iret got set
#endif
avail[*iret] = 0;
iret++;
}
}
AngularIntegration AngularIntegration::operator- (const AngularIntegration& angle) const {
dFloat sin_da = angle.m_sin_angle * m_cos_angle - angle.m_cos_angle * m_sin_angle;
dFloat cos_da = angle.m_cos_angle * m_cos_angle + angle.m_sin_angle * m_sin_angle;
return AngularIntegration(dAtan2(sin_da, cos_da));
}
void CustomUniversal::GetInfo (NewtonJointRecord* const info) const
{
strcpy (info->m_descriptionType, GetTypeName());
info->m_attachBody_0 = m_body0;
info->m_attachBody_1 = m_body1;
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
info->m_minLinearDof[0] = 0.0f;
info->m_maxLinearDof[0] = 0.0f;
info->m_minLinearDof[1] = 0.0f;
info->m_maxLinearDof[1] = 0.0f;;
info->m_minLinearDof[2] = 0.0f;
info->m_maxLinearDof[2] = 0.0f;
if (m_limit_0_On) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[0] = (m_minAngle_0 - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[0] = (m_maxAngle_0 - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[0] = -D_CUSTOM_LARGE_VALUE ;
info->m_maxAngularDof[0] = D_CUSTOM_LARGE_VALUE ;
}
// info->m_minAngularDof[1] = m_minAngle_1 * 180.0f / 3.141592f;
// info->m_maxAngularDof[1] = m_maxAngle_1 * 180.0f / 3.141592f;
if (m_limit_1_On) {
dFloat angle;
dFloat sinAngle;
dFloat cosAngle;
sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
angle = dAtan2 (sinAngle, cosAngle);
info->m_minAngularDof[1] = (m_minAngle_1 - angle) * 180.0f / 3.141592f ;
info->m_maxAngularDof[1] = (m_maxAngle_1 - angle) * 180.0f / 3.141592f ;
} else {
info->m_minAngularDof[1] = -D_CUSTOM_LARGE_VALUE ;
info->m_maxAngularDof[1] = D_CUSTOM_LARGE_VALUE ;
}
info->m_minAngularDof[2] = 0.0f;
info->m_maxAngularDof[2] = 0.0f;
memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
}