void DynamicRagDollJoint::SetConeAngle(dFloat angle)
{
m_coneAngle = angle;
m_coneAngleCos = dCos(angle);
m_coneAngleSin = dSin(angle);
m_coneAngleHalfCos = dCos(angle * 0.5f);
m_coneAngleHalfSin = dSin(angle * 0.5f);
}
void CustomLimitBallAndSocket::SetConeAngle (dFloat angle)
{
m_coneAngle = angle;
m_coneAngleCos = dCos (angle);
m_coneAngleSin = dSin (angle);
m_coneAngleHalfCos = dCos (angle * 0.5f);
m_coneAngleHalfSin = dSin (angle * 0.5f);
}
// this calculate the desired position and orientation in the path
dMatrix BuildMatrix (dFloat timestep)
{
dMatrix matrix (dPitchMatrix(-m_angle) * dRollMatrix(-m_angle));
matrix.m_posit = m_origin;
matrix.m_posit.m_z += radius * dSin (m_angle);
matrix.m_posit.m_y += radius * dCos (m_angle);
return matrix;
}
dMatrix dRollMatrix (dFloat ang)
{
dFloat cosAng;
dFloat sinAng;
sinAng = dSin (ang);
cosAng = dCos (ang);
return dMatrix (dVector ( cosAng, sinAng, 0.0f, 0.0f),
dVector (-sinAng, cosAng, 0.0f, 0.0f),
dVector ( 0.0f, 0.0f, 1.0f, 0.0f),
dVector ( 0.0f, 0.0f, 0.0f, 1.0f));
}
void CustomDGRayCastCar::SetTireSteerAngleForce (int index, dFloat angle, dFloat turnforce)
{
m_tires[index].m_steerAngle = angle;
m_tires[index].m_localAxis.m_z = dCos (angle);
m_tires[index].m_localAxis.m_x = dSin (angle);
// if (m_tiresRollSide==0) {
// m_tires[index].m_turnforce = turnforce;
// } else {
// m_tires[index].m_turnforce = -turnforce;
// }
}
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
dAASSERT (R);
sphi = dSin(phi);
cphi = dCos(phi);
stheta = dSin(theta);
ctheta = dCos(theta);
spsi = dSin(psi);
cpsi = dCos(psi);
_R(0,0) = cpsi*ctheta;
_R(0,1) = spsi*ctheta;
_R(0,2) =-stheta;
_R(0,3) = REAL(0.0);
_R(1,0) = cpsi*stheta*sphi - spsi*cphi;
_R(1,1) = spsi*stheta*sphi + cpsi*cphi;
_R(1,2) = ctheta*sphi;
_R(1,3) = REAL(0.0);
_R(2,0) = cpsi*stheta*cphi + spsi*sphi;
_R(2,1) = spsi*stheta*cphi - cpsi*sphi;
_R(2,2) = ctheta*cphi;
_R(2,3) = REAL(0.0);
}
EXPORT_C void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
sphi = dSin(phi);
cphi = dCos(phi);
stheta = dSin(theta);
ctheta = dCos(theta);
spsi = dSin(psi);
cpsi = dCos(psi);
_R(0,0) = dMUL(cpsi,ctheta);
_R(0,1) = dMUL(spsi,ctheta);
_R(0,2) =-stheta;
_R(0,3) = REAL(0.0);
_R(1,0) = dMUL(cpsi,dMUL(stheta,sphi)) - dMUL(spsi,cphi);
_R(1,1) = dMUL(spsi,dMUL(stheta,sphi)) + dMUL(cpsi,cphi);
_R(1,2) = dMUL(ctheta,sphi);
_R(1,3) = REAL(0.0);
_R(2,0) = dMUL(cpsi,dMUL(stheta,cphi)) + dMUL(spsi,sphi);
_R(2,1) = dMUL(spsi,dMUL(stheta,cphi)) - dMUL(cpsi,sphi);
_R(2,2) = dMUL(ctheta,cphi);
_R(2,3) = REAL(0.0);
}
dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
{
angle *= dFloat (0.5f);
m_q0 = dCos (angle);
dFloat sinAng = dSin (angle);
#ifdef _DEBUG
if (dAbs (angle) > dFloat(1.0e-6f)) {
dAssert (dAbs (dFloat(1.0f) - unitAxis % unitAxis) < dFloat(1.0e-3f));
}
#endif
m_q1 = unitAxis.m_x * sinAng;
m_q2 = unitAxis.m_y * sinAng;
m_q3 = unitAxis.m_z * sinAng;
}
void _InitCylinderTrimeshData(sData& cData)
{
// get cylinder information
// Rotation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
// Position
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
// Cylinder axis
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get trimesh position and orientation
const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
dMatrix3Copy(pRotTris,cData.mTrimeshRot);
dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
// Position
const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
dVector3Copy(*pPosTris,cData.vTrimeshPos);
// calculate basic angle for 8-gon
dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
// calculate angle increment
dReal fAngleIncrement = fAngle*REAL(2.0);
// calculate plane normals
// axis dependant code
for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = REAL(0.0);
fAngle += fAngleIncrement;
}
dSetZero(cData.vBestPoint,4);
// reset best depth
cData.fBestCenter = REAL(0.0);
}
CustomConeLimitedBallAndSocket::CustomConeLimitedBallAndSocket(
dFloat twistAngle,
dFloat coneAngle,
const dVector& coneDir,
const dVector& pivot,
NewtonBody* child,
NewtonBody* parent)
:CustomBallAndSocket(pivot, child, parent)
{
m_coneAngle = coneAngle;
m_coneAngle = twistAngle;
m_cosConeAngle = dCos (m_coneAngle);
// Recalculate local matrices so that the front vector align with the cone pin
CalculateLocalMatrix (pivot, coneDir, m_localMatrix0, m_localMatrix1);
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
rollPin = matrix1.RotateVector(rollPin);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(euler0.m_x);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
EXPORT_C void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dReal l = dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az);
if (l > REAL(0.0)) {
angle = dMUL(angle,REAL(0.5));
q[0] = dCos (angle);
l = dMUL(dReal(dSin(angle)),dRecipSqrt(l));
q[1] = dMUL(ax,l);
q[2] = dMUL(ay,l);
q[3] = dMUL(az,l);
}
else {
q[0] = REAL(1.0);
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dAASSERT (q);
dReal l = ax*ax + ay*ay + az*az;
if (l > REAL(0.0)) {
angle *= REAL(0.5);
q[0] = dCos (angle);
l = dSin(angle) * dRecipSqrt(l);
q[1] = ax*l;
q[2] = ay*l;
q[3] = az*l;
}
else {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
dCustomJoint::Debug(debugDisplay);
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1);
dMatrix coneRotation(dGetIdentityMatrix());
if (cosAngleCos < 0.9999f) {
dVector lateralDir(coneDir1.CrossProduct(coneDir0));
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
//dAssert(mag2 > 1.0e-4f);
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);
}
} else if (cosAngleCos < -0.9999f) {
coneRotation[0][0] = -1.0f;
coneRotation[1][1] = -1.0f;
}
const int subdiv = 18;
const dFloat radius = debugDisplay->m_debugScale;
dVector arch[subdiv + 1];
// show twist angle limits
if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) {
dMatrix pitchMatrix(matrix1 * coneRotation);
pitchMatrix.m_posit = matrix1.m_posit;
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv;
dFloat angle0 = m_minTwistAngle;
debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
// show cone angle limits
if (m_options.m_option2) {
dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f);
dFloat angleStep = dPi * 2.0f / subdiv;
dFloat angle0 = 0.0f;
debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
dVector conePoint(dPitchMatrix(angle0).RotateVector(point));
dVector p(matrix1.TransformVector(conePoint));
arch[i] = p;
debugDisplay->DrawLine(matrix1.m_posit, p);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
void CustomDGRayCastCar::AddSingleSuspensionTire (
void *userData,
const dVector& localPosition,
dFloat mass,
dFloat radius,
dFloat width,
dFloat friction,
dFloat suspensionLenght,
dFloat springConst,
dFloat springDamper,
int castMode)
{
dVector relTirePos = localPosition;
suspensionLenght = dAbs ( suspensionLenght );
dMatrix chassisMatrix;
NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]);
chassisMatrix = chassisMatrix * chassisMatrix;
relTirePos += chassisMatrix.m_up.Scale ( suspensionLenght );
m_tires[m_tiresCount].m_harpoint = m_localFrame.UntransformVector( relTirePos );
m_tires[m_tiresCount].m_localAxis = dVector (0.0f, 0.0f, 1.0f, 0.0f);
m_tires[m_tiresCount].m_tireAxelPosit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_tireAxelVeloc = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_lateralPin = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_longitudinalPin = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
m_tires[m_tiresCount].m_HitBody = NULL;
m_tires[m_tiresCount].m_userData = userData;
m_tires[m_tiresCount].m_spinAngle = 0.0f;
m_tires[m_tiresCount].m_steerAngle = 0.0f;
m_tires[m_tiresCount].m_tireLoad = 0.0f;
m_tires[m_tiresCount].m_posit = suspensionLenght;
m_tires[m_tiresCount].m_tireIsOnAir = 1;
m_tires[m_tiresCount].m_tireUseConvexCastMode = castMode;
m_tires[m_tiresCount].m_springConst = springConst;
m_tires[m_tiresCount].m_springDamper = springDamper;
m_tires[m_tiresCount].m_suspensionLenght = suspensionLenght;
m_tires[m_tiresCount].m_angularVelocity = 0.0f;
m_tires[m_tiresCount].m_breakForce = 0.0f;
m_tires[m_tiresCount].m_torque = 0.0f;
m_tires[m_tiresCount].m_groundFriction = friction;
m_tires[m_tiresCount].m_tireIsConstrained = 0;
m_tires[m_tiresCount].m_mass = mass;
m_tires[m_tiresCount].m_width = width;
m_tires[m_tiresCount].m_radius = radius;
m_tires[m_tiresCount].m_Ixx = mass * radius * radius / 2.0f;
m_tires[m_tiresCount].m_IxxInv = 1.0f / m_tires[m_tiresCount].m_Ixx;
#define TIRE_SHAPE_SIZE 12
dVector shapePoints[TIRE_SHAPE_SIZE * 2];
for ( int i = 0; i < TIRE_SHAPE_SIZE; i ++ ) {
shapePoints[i].m_x = -width * 0.5f;
shapePoints[i].m_y = radius * dCos ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
shapePoints[i].m_z = radius * dSin ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
shapePoints[i + TIRE_SHAPE_SIZE].m_x = -shapePoints[i].m_x;
shapePoints[i + TIRE_SHAPE_SIZE].m_y = shapePoints[i].m_y;
shapePoints[i + TIRE_SHAPE_SIZE].m_z = shapePoints[i].m_z;
}
m_tires[m_tiresCount].m_shape = NewtonCreateConvexHull ( m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, 0, NULL );
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateSphere(m_world,radius,radius,radius,&offmat[0][0]);
// NewtonCreateCone(m_world,radius,width,NULL);
// NewtonCreateCapsule(m_world,radius,width,NULL);
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateCylinder(m_world,radius*2,width*2,NULL);
// NewtonCreateBox(m_world,radius*2,radius*2,radius*2,NULL);
// NewtonCreateConvexHull (m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, NULL);
m_tiresCount ++;
}
void dxStepBody (dxBody *b, dReal h)
{
// cap the angular velocity
if (b->flags & dxBodyMaxAngularSpeed) {
const dReal max_ang_speed = b->max_angular_speed;
const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
if (aspeed > max_ang_speed*max_ang_speed) {
const dReal coef = max_ang_speed/dSqrt(aspeed);
dScaleVector3(b->avel, coef);
}
}
// end of angular velocity cap
// handle linear velocity
for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (unsigned int j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
dxWorldProcessContext *world_process_context = b->world->UnsafeGetWorldProcessingContext();
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
world_process_context->LockForStepbodySerialization();
dGeomMoved (geom);
world_process_context->UnlockForStepbodySerialization();
}
// notify the user
if (b->moved_callback != NULL) {
b->moved_callback(b);
}
// damping
if (b->flags & dxBodyLinearDamping) {
const dReal lin_threshold = b->dampingp.linear_threshold;
const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
if ( lin_speed > lin_threshold) {
const dReal k = 1 - b->dampingp.linear_scale;
dScaleVector3(b->lvel, k);
}
}
if (b->flags & dxBodyAngularDamping) {
const dReal ang_threshold = b->dampingp.angular_threshold;
const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
if ( ang_speed > ang_threshold) {
const dReal k = 1 - b->dampingp.angular_scale;
dScaleVector3(b->avel, k);
}
}
}
void dxStepBody (dxBody *b, dReal h)
{
int j;
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
// handle linear velocity
for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->R);
// notify all attached geoms that this body has moved
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
}
// initialize collision data
void _cldInitCylinderBox(sCylinderBoxData& cData)
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(cData.gBox);
dMatrix3Copy(pRotBox,cData.mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
dVector3Copy(*pPosBox,cData.vBoxPos);
dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
cData.vBoxHalfSize[0] *= REAL(0.5);
cData.vBoxHalfSize[1] *= REAL(0.5);
cData.vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
// vertex 1
cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
// vertex 2
cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
// vertex 3
cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
// vertex 4
cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
// vertex 5
cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
// vertex 6
cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
// vertex 7
cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
}
// find relative position
dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
cData.fBestDepth = MAX_FLOAT;
cData.vNormal[0] = REAL(0.0);
cData.vNormal[1] = REAL(0.0);
cData.vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = M_PI/nCYLINDER_SEGMENT;
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
cData.fBestrb = 0;
cData.fBestrc = 0;
cData.iBestAxis = 0;
cData.nContacts = 0;
}
//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::SetTireSteerAngleForce (int index, dFloat angle, dFloat turnforce)
{
m_tires[index].m_steerAngle = angle;
m_tires[index].m_localAxisInJointSpace.m_z = dCos (angle);
m_tires[index].m_localAxisInJointSpace.m_x = dSin (angle);
}
// initialize collision data
void sCylinderBoxData::_cldInitCylinderBox()
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(m_gCylinder);
dMatrix3Copy(pRotCyc,m_mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder);
dVector3Copy(*pPosCyc,m_vCylinderPos);
dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(m_gBox);
dMatrix3Copy(pRotBox,m_mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(m_gBox);
dVector3Copy(*pPosBox,m_vBoxPos);
dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize);
m_vBoxHalfSize[0] *= REAL(0.5);
m_vBoxHalfSize[1] *= REAL(0.5);
m_vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
m_avBoxVertices[0][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[0][1] = m_vBoxHalfSize[1];
m_avBoxVertices[0][2] = -m_vBoxHalfSize[2];
// vertex 1
m_avBoxVertices[1][0] = m_vBoxHalfSize[0];
m_avBoxVertices[1][1] = m_vBoxHalfSize[1];
m_avBoxVertices[1][2] = -m_vBoxHalfSize[2];
// vertex 2
m_avBoxVertices[2][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[2][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[2][2] = -m_vBoxHalfSize[2];
// vertex 3
m_avBoxVertices[3][0] = m_vBoxHalfSize[0];
m_avBoxVertices[3][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[3][2] = -m_vBoxHalfSize[2];
// vertex 4
m_avBoxVertices[4][0] = m_vBoxHalfSize[0];
m_avBoxVertices[4][1] = m_vBoxHalfSize[1];
m_avBoxVertices[4][2] = m_vBoxHalfSize[2];
// vertex 5
m_avBoxVertices[5][0] = m_vBoxHalfSize[0];
m_avBoxVertices[5][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[5][2] = m_vBoxHalfSize[2];
// vertex 6
m_avBoxVertices[6][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[6][1] = -m_vBoxHalfSize[1];
m_avBoxVertices[6][2] = m_vBoxHalfSize[2];
// vertex 7
m_avBoxVertices[7][0] = -m_vBoxHalfSize[0];
m_avBoxVertices[7][1] = m_vBoxHalfSize[1];
m_avBoxVertices[7][2] = m_vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]);
}
// find relative position
dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff);
m_fBestDepth = MAX_FLOAT;
m_vNormal[0] = REAL(0.0);
m_vNormal[1] = REAL(0.0);
m_vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT);
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
m_avCylinderNormals[i][0] = -dCos(fAngle);
m_avCylinderNormals[i][1] = -dSin(fAngle);
m_avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
m_fBestrb = 0;
m_fBestrc = 0;
m_iBestAxis = 0;
m_nContacts = 0;
}
static inline void
moveAndRotateBody (dxBody * b, dReal h)
{
int j;
// handle linear velocity
for (j = 0; j < 3; j++)
b->posr.pos[j] += dMUL(h,b->lvel[j]);
if (b->flags & dxBodyFlagFiniteRotation)
{
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis, b->avel);
frv[0] = dMUL(b->finite_rot_axis[0],k);
frv[1] = dMUL(b->finite_rot_axis[1],k);
frv[2] = dMUL(b->finite_rot_axis[2],k);
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(k,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(frv[0],s);
q[2] = dMUL(frv[1],s);
q[3] = dMUL(frv[2],s);
}
else
{
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (dMUL(b->avel[0],b->avel[0]) + dMUL(b->avel[1],b->avel[1]) + dMUL(b->avel[2],b->avel[2]));
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(wlen,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(b->avel[0],s);
q[2] = dMUL(b->avel[1],s);
q[3] = dMUL(b->avel[2],s);
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2, q, b->q);
for (j = 0; j < 4; j++)
b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
dReal dq[4];
dWtoDQ (irv, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
}
else
{
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q, b->posr.R);
// notify all attached geoms that this body has moved
for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
}
dFloat AngularIntegration::update(dFloat angle) {
return update(dCos(angle), dSin(angle));
}
void AngularIntegration::set_angle(dFloat angle) {
m_angle = angle;
m_sin_angle = dSin(angle);
m_cos_angle = dCos(angle);
}
