void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// If we're in Euler mode, joint->axis[1] doesn't
// have anything sensible in it. So don't just return
// that, find the actual effective axis.
// Likewise, the actual axis of rotation for the
// the other axes is different from what's stored.
if ( joint->mode == dAMotorEuler ) {
dVector3 axes[3];
joint->computeGlobalAxes(axes);
if (anum == 1) {
result[0]=axes[1][0];
result[1]=axes[1][1];
result[2]=axes[1][2];
} else if (anum == 0) {
// This won't be unit length in general,
// but it's what's used in getInfo2
// This may be why things freak out as
// the body-relative axes get close to each other.
dCalcVectorCross3( result, axes[1], axes[2] );
} else if (anum == 2) {
// Same problem as above.
dCalcVectorCross3( result, axes[0], axes[1] );
}
} else if ( joint->rel[anum] > 0 ) {
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
// helper for less key strokes
inline void _CalculateAxis(const dVector3& v1,
const dVector3& v2,
const dVector3& v3,
const dVector3& v4,
dVector3& r)
{
dVector3 t1;
dVector3 t2;
SUBTRACT(v1,v2,t1);
dCalcVectorCross3(t2,t1,v3);
dCalcVectorCross3(r,t2,v4);
}
void
dxJointHinge2::makeW1andW2()
{
if ( node[1].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, w;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;
// modify axis 1 so it's perpendicular to axis 2
dReal k = dCalcVectorDot3( ax2, ax1 );
for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
dNormalize3( ax1 );
// make w1 = modified axis1, w2 = axis2 x (modified axis1)
dCalcVectorCross3( w, ax2, ax1 );
dMultiply1_331( w1, node[1].body->posr.R, ax1 );
dMultiply1_331( w2, node[1].body->posr.R, w );
}
}
void
dxJointHinge2::makeV1andV2()
{
if ( node[0].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, v;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
(_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
(_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
return;
// modify axis 2 so it's perpendicular to axis 1
dReal k = dCalcVectorDot3( ax1, ax2 );
for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
dNormalize3( ax2 );
// make v1 = modified axis2, v2 = axis1 x (modified axis2)
dCalcVectorCross3( v, ax1, ax2 );
dMultiply1_331( v1, node[0].body->posr.R, ax2 );
dMultiply1_331( v2, node[0].body->posr.R, v );
}
}
static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
dVector3 p1, dVector3 p2, dVector3 p3)
{
int k;
dVector3 u1,u2,n,tmp;
for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
dReal d1 = dSqrt(dCalcVectorDot3(u1,u1));
dReal d2 = dSqrt(dCalcVectorDot3(u2,u2));
dNormalize3 (u1);
dNormalize3 (u2);
if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
dCalcVectorCross3(n,u1,u2);
for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
dReal d = -dCalcVectorDot3(n,p1);
if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp);
for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
if (alpha < 0) return 0;
if (alpha > 1) return 0;
for (k=0; k<3; k++) tmp[k] -= p1[k];
dReal a1 = dCalcVectorDot3(u1,tmp);
dReal a2 = dCalcVectorDot3(u2,tmp);
if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
return 1;
}
void
dxJointAMotor::getInfo2( dxJoint::Info2 *info )
{
int i;
// compute the axes (if not global)
dVector3 ax[3];
computeGlobalAxes( ax );
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt = 0 ax[1]
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
// to prove the result for angle[0], write the expression for angle[0] from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
dVector3 *axptr[3];
axptr[0] = &ax[0];
axptr[1] = &ax[1];
axptr[2] = &ax[2];
dVector3 ax0_cross_ax1;
dVector3 ax1_cross_ax2;
if ( mode == dAMotorEuler )
{
dCalcVectorCross3( ax0_cross_ax1, ax[0], ax[1] );
axptr[2] = &ax0_cross_ax1;
dCalcVectorCross3( ax1_cross_ax2, ax[1], ax[2] );
axptr[0] = &ax1_cross_ax2;
}
int row = 0;
for ( i = 0; i < num; i++ )
{
row += limot[i].addLimot( this, info, row, *( axptr[i] ), 1 );
}
}
////////////////////////////////////////////////////////////////////////////////
/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
/// relative to body 1 and 2 initially) and then computes the constrained
/// rotational axis as the cross product of ax1 and ax2.
/// the sin and cos of the angle between axis 1 and 2 is computed, this comes
/// from dot and cross product rules.
///
/// @param ax1 Will contain the joint axis1 in world frame
/// @param ax2 Will contain the joint axis2 in world frame
/// @param axis Will contain the cross product of ax1 x ax2
/// @param sin_angle
/// @param cos_angle
////////////////////////////////////////////////////////////////////////////////
void
dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
dReal &sin_angle, dReal &cos_angle) const
{
dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
dCalcVectorCross3(axCross,ax1,ax2);
sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
cos_angle = dCalcVectorDot3 (ax1,ax2);
}
// compute the 3 axes in global coordinates
void
dxJointAMotor::computeGlobalAxes( dVector3 ax[3] )
{
if ( mode == dAMotorEuler )
{
// special handling for euler mode
dMultiply0_331( ax[0], node[0].body->posr.R, axis[0] );
if ( node[1].body )
{
dMultiply0_331( ax[2], node[1].body->posr.R, axis[2] );
}
else
{
ax[2][0] = axis[2][0];
ax[2][1] = axis[2][1];
ax[2][2] = axis[2][2];
}
dCalcVectorCross3( ax[1], ax[2], ax[0] );
dNormalize3( ax[1] );
}
else
{
for ( int i = 0; i < num; i++ )
{
if ( rel[i] == 1 )
{
// relative to b1
dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
}
else if ( rel[i] == 2 )
{
// relative to b2
if ( node[1].body ) // jds: don't assert, just ignore
{
dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
}
else
{
// global - just copy it
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
else
{
// global - just copy it
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
}
}
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 ) );
}
void makeRandomRotation (dMatrix3 R)
{
dReal *u1 = R, *u2=R+4, *u3=R+8;
dMakeRandomVector (u1,3,1.0);
dNormalize3 (u1);
dMakeRandomVector (u2,3,1.0);
dReal d = dCalcVectorDot3(u1,u2);
u2[0] -= d*u1[0];
u2[1] -= d*u1[1];
u2[2] -= d*u1[2];
dNormalize3(u2);
dCalcVectorCross3(u3,u1,u2);
}
void dRigidBodyArraySetAngularVel(dRigidBodyArrayID bodyArray, dReal ax, dReal ay, dReal az) {
dBodyID center = bodyArray->center;
const dReal *p0 = dBodyGetPosition(center);
dVector3 omega = {ax, ay, az};
for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) {
dBodyID body = dRigidBodyArrayGet(bodyArray, i);
const dReal *p = dBodyGetPosition(body);
dVector3 pdot, r;
dOP(r, -, p, p0);
dCalcVectorCross3(pdot, omega, r);
dBodySetLinearVel(body, pdot[0], pdot[1], pdot[2]);
dBodySetAngularVel(body, ax, ay, az);
}
}
void testCrossProduct()
{
HEADER;
dVector3 a1,a2,b,c;
dMatrix3 B;
dMakeRandomVector (b,3,1.0);
dMakeRandomVector (c,3,1.0);
dCalcVectorCross3(a1,b,c);
dSetZero (B,12);
dSetCrossMatrixPlus(B,b,4);
dMultiply0 (a2,B,c,3,3,1);
dReal diff = dMaxDifference(a1,a2,3,1);
printf ("\t%s\n", diff > tol ? "FAILED" : "passed");
}
void dJointAddScrewForce ( dJointID j, dReal force )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dVector3 axis;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
if ( joint->flags & dJOINT_REVERSE )
force -= force;
getAxis ( joint, axis, joint->axis1 );
axis[0] *= force;
axis[1] *= force;
axis[2] *= force;
if ( joint->node[0].body != 0 )
dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
if ( joint->node[1].body != 0 )
dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
{
// linear torque decoupling:
// we have to compensate the torque, that this screw force may generate
// if body centers are not aligned along the screw axis
dVector3 ltd; // Linear Torque Decoupling vector (a torque)
dVector3 cgdiff;
cgdiff[0] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[0] -
joint->node[0].body->posr.pos[0] );
cgdiff[1] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[1] -
joint->node[0].body->posr.pos[1] );
cgdiff[2] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[2] -
joint->node[0].body->posr.pos[2] );
dCalcVectorCross3( ltd, cgdiff, axis );
dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
}
}
/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dCalcVectorLengthSquare3(m);
if (n0 != 1)
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dCalcVectorDot3(m, m+4);
if (proj != 0) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dCalcVectorLengthSquare3(m+4);
if (n1 != 1)
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCalcVectorCross3(m+8, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}
void
dxJointHinge::getInfo2( dxJoint::Info2 *info )
{
// Added by OSRF
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
info->cfm[5] = cfm;
}
// set the three ball-and-socket rows
setBall( this, info, anchor1, anchor2 );
// set the two hinge rows. the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
dVector3 p, q; // plane space vectors for ax1
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dPlaneSpace( ax1, p, q );
// strange the rotation matrix is not really a rotation matrix (non-orthogonal vectors)
// normals of columns and rows are not exactly 1 when velocity is large.
// printf("posr.R\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
// node[0].body->posr.R[0*4+0],node[0].body->posr.R[0*4+1],node[0].body->posr.R[0*4+2],node[0].body->posr.R[0*4+3],
// node[0].body->posr.R[1*4+0],node[0].body->posr.R[1*4+1],node[0].body->posr.R[1*4+2],node[0].body->posr.R[1*4+3],
// node[0].body->posr.R[2*4+0],node[0].body->posr.R[2*4+1],node[0].body->posr.R[2*4+2],node[0].body->posr.R[2*4+3]);
// printf("axis1 [%f %f %f] ax1 [%f %f %f]\n",
// axis1[0], axis1[1], axis1[2],
// ax1[0], ax1[1], ax1[2]);
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
info->J1a[s4+0] = q[0];
info->J1a[s4+1] = q[1];
info->J1a[s4+2] = q[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
info->J2a[s4+0] = -q[0];
info->J2a[s4+1] = -q[1];
info->J2a[s4+2] = -q[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
dVector3 ax2, b;
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
dCalcVectorCross3( b, ax1, ax2 );
dReal k = info->fps * info->erp;
info->c[3] = k * dCalcVectorDot3( b, p );
info->c[4] = k * dCalcVectorDot3( b, q );
// if the hinge is powered, or has joint limits, add in the stuff
limot.addLimot( this, info, 5, ax1, 1 );
// joint damping
if (this->use_damping)
{
// added J1ad and J2ad for damping, only 1 row
info->J1ad[0] = ax1[0];
info->J1ad[1] = ax1[1];
info->J1ad[2] = ax1[2];
if ( this->node[1].body )
{
info->J2ad[0] = -ax1[0];
info->J2ad[1] = -ax1[1];
info->J2ad[2] = -ax1[2];
}
// there's no rhs for damping setup, all we want to use is the jacobian information above
}
}
void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
// body of the whole vehicle
dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody;
unsigned long geom1Categories = dGeomGetCategoryBits(o1);
// speeds of the belts
const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity();
const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity();
dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now
if (geom1Categories & Category::LEFT) {
beltSpeed = leftBeltSpeed;
} else {
beltSpeed = rightBeltSpeed;
}
// the desired linear and angular speeds (set by desired track velocities)
const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2;
const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance;
// radius of the turn the robot is doing
const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ?
dInfinity : // is driving straight
((fabs(linearSpeed) < 0.1) ?
0 : // is rotating about a single point
linearSpeed / angularSpeed // general movement
);
dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis
dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case
{ // compute the center of rotation
dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal);
dCopyVector3(centerOfRotation, yAxisGlobal);
// make the unit vector as long as we need (and change orientation if needed; the radius is a signed number)
dScaleVector3(centerOfRotation, desiredRotationRadiusSigned);
const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody);
dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos);
}
int maxContacts = 20;
dContact contact[maxContacts];
int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
for(size_t i = 0; i < numContacts; i++) {
dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body
dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos);
dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction
dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal);
if (beltSpeed > 0) {
dNegateVector3(beltDirection);
}
if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive
dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point
dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation);
// the friction force should be perpendicular to COR2Contact
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact);
const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1;
// contactInVehiclePos[0] > 0 means the contact is in the front part of the track
if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) !=
sgn(contactInVehiclePos[0]) * linearSpeedSignum) {
dNegateVector3(contact[i].fdir1);
}
} else { // straight drive
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal);
if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) {
dNegateVector3(contact[i].fdir1);
}
}
// use friction direction and motion1 to simulate the track movement
contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2;
contact[i].surface.mu = 0.5;
contact[i].surface.mu2 = 10;
// the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors)
contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07;
// friction force visualization
dMatrix3 forceRotation;
dVector3 vec;
dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec);
dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]);
posr data;
dCopyVector3(data.pos, contact[i].geom.pos);
dCopyMatrix4x3(data.R, forceRotation);
forces.push_back(data);
dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]);
dJointAttach(c, b1, b2);
if(!isValidCollision(o1, o2, contact[i]))
this->badCollision = true;
if(config.contact_grouser_terrain.debug)
this->contacts.push_back(contact[i].geom);
}
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
dCopyVector3(Sphere.mCenter, Position);
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
dSubtractVectors3r4(vu, v1, v0);
vu[3] = REAL(0.0);
dVector3 vv;
dSubtractVectors3r4(vv, v2, v0);
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCalcVectorCross3(Plane, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dCalcVectorDot3(Plane, Position) - dCalcVectorDot3(Plane, v0);
if(side < REAL(0.0))
{
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
dAddScaledVectors3r4(ContactPos, v1, v2, u, v);
dAddScaledVector3r4(ContactPos, v0, w);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dSubtractVectors3r4(dir, Position, ContactPos);
dReal dirProj = dCalcVectorDot3(dir, Plane) / dCalcVectorLength3(dir);
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0))
continue; // Zero contact depth could be ignored
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dCopyVector3r4(Contact->pos, ContactPos);
// Using normal as plane (reversed)
dCopyNegatedVector3r4(Contact->normal, Plane);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
// We need to set these unconditionally, as the merging may fail! - Bram
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
Contact->side1 = TriIndex;
OutTriCount++;
}
if (OutTriCount > 0)
{
if (TriMesh->SphereContactsMergeOption == MERGE_CONTACTS_FULLY)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 pos;
dCopyVector3r4(pos, Contact->pos);
dVector3 normal;
dCopyScaledVector3r4(normal, Contact->normal, Contact->depth);
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddVector3r4(pos, TempContact->pos);
dAddScaledVector3r4(normal, TempContact->normal, TempContact->depth);
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
dReal invOutTriCount = dRecip(OutTriCount);
dCopyScaledVector3r4(Contact->pos, pos, invOutTriCount);
if ( !dSafeNormalize3(normal) )
return OutTriCount; // Cannot merge in this pathological case
// Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
// That is why we need to correct this by increasing the depth for each intersection.
// The maximum of the adjusted depths is our newly merged depth value - Bram.
dReal mergedDepth = REAL(0.0);
dReal minEffectiveness = REAL(0.5);
for ( int i = 0; i < OutTriCount; ++i )
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
if ( effectiveness < dEpsilon )
return OutTriCount; // Cannot merge this pathological case
// Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
dReal adjusted = TempContact->depth / effectiveness;
mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
}
Contact->depth = mergedDepth;
dCopyVector3r4(Contact->normal, normal);
}
return 1;
}
else if (TriMesh->SphereContactsMergeOption == MERGE_CONTACT_NORMALS)
{
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
dCopyScaledVector3r4(Normal, FirstContact->normal, FirstContact->depth);
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddScaledVector3r4(Normal, Contact->normal, Contact->depth);
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dCopyVector3r4(Contact->normal, Normal);
}
}
return OutTriCount;
}
else
{
dIASSERT(TriMesh->SphereContactsMergeOption == DONT_MERGE_CONTACTS);
return OutTriCount;
}
}
else return 0;
}
else return 0;
}
void
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
const dReal k = worldFPS * worldERP;
// ======================================================================
// The angular constraint
//
dVector3 ax1, ax2; // Global axes of rotation
getAxis(this, ax1, axis1);
getAxis2(this,ax2, axis2);
dVector3 uniPerp; // Axis perpendicular to axes of rotation
dCalcVectorCross3(uniPerp,ax1,ax2);
dNormalize3( uniPerp );
dCopyVector3( J1 + GI2__JA_MIN, uniPerp );
dxBody *body1 = node[1].body;
if ( body1 ) {
dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp );
}
// Corrective velocity attempting to keep uni axes perpendicular
dReal val = dCalcVectorDot3( ax1, ax2 );
// Small angle approximation :
// theta = asin(val)
// theta is approximately val when val is near zero.
pairRhsCfm[GI2_RHS] = -k * val;
// ==========================================================================
// Handle axes orthogonal to the prismatic
dVector3 an1, an2; // Global anchor positions
dVector3 axP, sep; // Prismatic axis and separation vector
getAnchor(this, an1, anchor1);
getAnchor2(this, an2, anchor2);
if (flags & dJOINT_REVERSE) {
getAxis2(this, axP, axisP1);
} else {
getAxis(this, axP, axisP1);
}
dSubtractVectors3(sep, an2, an1);
dVector3 p, q;
dPlaneSpace(axP, p, q);
dCopyVector3( J1 + rowskip + GI2__JL_MIN, p );
dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q );
// Make the anchors be body local
// Aliasing isn't a problem here.
dSubtractVectors3(an1, an1, node[0].body->posr.pos);
dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p );
dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q );
if (body1) {
dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p );
dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q );
dSubtractVectors3(an2, an2, body1->posr.pos);
dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 );
dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 );
}
pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep );
pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep );
// ==========================================================================
// Handle the limits/motors
int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip;
if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) {
currRowSkip += rowskip; currPairSkip += pairskip;
}
if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) {
currRowSkip += rowskip; currPairSkip += pairskip;
}
if ( body1 || (flags & dJOINT_REVERSE) == 0 ) {
limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
} else {
dNegateVector3(axP);
limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
}
}
void
dxJointUniversal::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
// set the three ball-and-socket rows
setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the constraint
// equation is
// p*w1 - p*w2 = 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
// length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
// about this.
dVector3 p;
// Since axis1 and axis2 may not be perpendicular
// we find a axis2_tmp which is really perpendicular to axis1
// and in the plane of axis1 and axis2
getAxes( ax1, ax2 );
dReal k = dCalcVectorDot3( ax1, ax2 );
dVector3 ax2_temp;
dAddVectorScaledVector3(ax2_temp, ax2, ax1, -k);
dCalcVectorCross3( p, ax1, ax2_temp );
dNormalize3( p );
int currRowSkip = 3 * rowskip;
{
dCopyVector3( J1 + currRowSkip + GI2__JA_MIN, p);
if ( node[1].body )
{
dCopyNegatedVector3( J2 + currRowSkip + GI2__JA_MIN, p);
}
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
int currPairSkip = 3 * pairskip;
{
pairRhsCfm[currPairSkip + GI2_RHS] = worldFPS * worldERP * (-k);
}
currRowSkip += rowskip; currPairSkip += pairskip;
// if the first angle is powered, or has joint limits, add in the stuff
if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 ))
{
currRowSkip += rowskip; currPairSkip += pairskip;
}
// if the second angle is powered, or has joint limits, add in more stuff
limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 );
}
void
dxJointTransmission::getInfo2( dReal worldFPS,
dReal /*worldERP*/,
const Info2Descr* info )
{
dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
const dReal *p[2], *omega[2];
int i;
// Transform all needed quantities to the global frame.
for (i = 0 ; i < 2 ; i += 1) {
dBodyGetRelPointPos(node[i].body,
anchors[i][0], anchors[i][1], anchors[i][2],
a[i]);
dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
n[i]);
p[i] = dBodyGetPosition(node[i].body);
omega[i] = dBodyGetAngularVel(node[i].body);
}
if (update) {
// Make sure both gear reference frames end up with the same
// handedness.
if (dCalcVectorDot3(n[0], n[1]) < 0) {
dNegateVector3(axes[0]);
dNegateVector3(n[0]);
}
}
// Calculate the mesh geometry based on the current mode.
switch (mode) {
case dTransmissionParallelAxes:
// Simply calculate the contact point as the point on the
// baseline that will yield the correct ratio.
dIASSERT (ratio > 0);
dSubtractVectors3(d, a[1], a[0]);
dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio));
dCopyVector3(c[1], c[0]);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
dCalcVectorCross3(l[i], d, n[i]);
}
break;
case dTransmissionIntersectingAxes:
// Calculate the line of intersection between the planes of the
// gears.
dCalcVectorCross3(l[0], n[0], n[1]);
dCopyVector3(l[1], l[0]);
nn = dCalcVectorDot3(n[0], n[1]);
dIASSERT(fabs(nn) != 1);
na_0 = dCalcVectorDot3(n[0], a[0]);
na_1 = dCalcVectorDot3(n[1], a[1]);
dAddScaledVectors3(O, n[0], n[1],
(na_0 - na_1 * nn) / (1 - nn * nn),
(na_1 - na_0 * nn) / (1 - nn * nn));
// Find the contact point as:
//
// c = ((r_a - O) . l) l + O
//
// where r_a the anchor point of either gear and l, O the tangent
// line direction and origin.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3(d, a[i], O);
m = dCalcVectorDot3(d, l[i]);
dAddScaledVectors3(c[i], O, l[i], 1, m);
}
break;
case dTransmissionChainDrive:
dSubtractVectors3(d, a[0], a[1]);
m = dCalcVectorLength3(d);
dIASSERT(m > 0);
// Caclulate the angle of the contact point relative to the
// baseline.
cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
// Calculate the contact radius in the local reference
// frame of the chain. This has axis x pointing along the
// baseline, axis y pointing along the sprocket axis and
// the remaining axis normal to both.
u[0] = radii[i] * cosphi;
u[1] = 0;
u[2] = radii[i] * sinphi;
// Transform the contact radius into the global frame.
dCalcVectorCross3(z, d, n[i]);
v[0] = dCalcVectorDot3(d, u);
v[1] = dCalcVectorDot3(n[i], u);
v[2] = dCalcVectorDot3(z, u);
// Finally calculate contact points and l.
dAddVectors3(c[i], a[i], v);
dCalcVectorCross3(l[i], v, n[i]);
dNormalize3(l[i]);
// printf ("%d: %f, %f, %f\n",
// i, l[i][0], l[i][1], l[i][2]);
}
break;
}
if (update) {
// We need to calculate an initial reference frame for each
// wheel which we can measure the current phase against. This
// frame will have the initial contact radius as the x axis,
// the wheel axis as the z axis and their cross product as the
// y axis.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], a[i]);
radii[i] = dCalcVectorLength3(r[i]);
dIASSERT(radii[i] > 0);
dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
reference[i]);
dNormalize3(reference[i]);
dCopyVector3(reference[i] + 8, axes[i]);
dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);
// printf ("%f\n", dDOT(r[i], n[i]));
// printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
// reference[i][0],reference[i][1],reference[i][2],
// reference[i][4],reference[i][5],reference[i][6],
// reference[i][8],reference[i][9],reference[i][10]);
radii[i] = radii[i];
phase[i] = 0;
}
ratio = radii[0] / radii[1];
update = 0;
}
for (i = 0 ; i < 2 ; i += 1) {
dReal phase_hat;
dSubtractVectors3 (r[i], c[i], a[i]);
// Transform the (global) contact radius into the gear's
// reference frame.
dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
dMultiply0_331(t, reference[i], s);
// Now simply calculate its angle on the plane relative to the
// x-axis which is the initial contact radius. This will be
// an angle between -pi and pi that is coterminal with the
// actual phase of the wheel. To find the real phase we
// estimate it by adding omega * dt to the old phase and then
// find the closest angle to that, that is coterminal to
// theta.
theta = atan2(t[1], t[0]);
phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;
if (phase_hat > M_PI_2) {
if (theta < 0) {
theta += (dReal)(2 * M_PI);
}
theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
} else if (phase_hat < -M_PI_2) {
if (theta > 0) {
theta -= (dReal)(2 * M_PI);
}
theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
}
if (phase_hat - theta > M_PI) {
phase[i] = theta + (dReal)(2 * M_PI);
} else if (phase_hat - theta < -M_PI) {
phase[i] = theta - (dReal)(2 * M_PI);
} else {
phase[i] = theta;
}
dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
}
// Calculate the phase error. Depending on the mode the condition
// is that the distances traveled by each contact point must be
// either equal (chain and sprockets) or opposite (gears).
if (mode == dTransmissionChainDrive) {
delta = (dCalcVectorLength3(r[0]) * phase[0] -
dCalcVectorLength3(r[1]) * phase[1]);
} else {
delta = (dCalcVectorLength3(r[0]) * phase[0] +
dCalcVectorLength3(r[1]) * phase[1]);
}
// When in chain mode a torque reversal, signified by the change
// in sign of the wheel phase difference, has the added effect of
// switching the active chain branch. We must therefore reflect
// the contact points and tangents across the baseline.
if (mode == dTransmissionChainDrive && delta < 0) {
dVector3 d;
dSubtractVectors3(d, a[0], a[1]);
for (i = 0 ; i < 2 ; i += 1) {
dVector3 nn;
dReal a;
dCalcVectorCross3(nn, n[i], d);
a = dCalcVectorDot3(nn, nn);
dIASSERT(a > 0);
dAddScaledVectors3(c[i], c[i], nn,
1, -2 * dCalcVectorDot3(c[i], nn) / a);
dAddScaledVectors3(l[i], l[i], nn,
-1, 2 * dCalcVectorDot3(l[i], nn) / a);
}
}
// Do not add the constraint if there's backlash and we're in the
// backlash gap.
if (backlash == 0 || fabs(delta) > backlash) {
// The constraint is satisfied iff the absolute velocity of the
// contact point projected onto the tangent of the wheels is equal
// for both gears. This velocity can be calculated as:
//
// u = v + omega x r_c
//
// The constraint therefore becomes:
// (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
// (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], p[i]);
}
dCalcVectorCross3(info->J1a, r[0], l[0]);
dCalcVectorCross3(info->J2a, l[1], r[1]);
dCopyVector3(info->J1l, l[0]);
dCopyNegatedVector3(info->J2l, l[1]);
if (delta > 0) {
if (backlash > 0) {
info->lo[0] = -dInfinity;
info->hi[0] = 0;
}
info->c[0] = -worldFPS * erp * (delta - backlash);
} else {
if (backlash > 0) {
info->lo[0] = 0;
info->hi[0] = dInfinity;
}
info->c[0] = -worldFPS * erp * (delta + backlash);
}
}
info->cfm[0] = cfm;
// printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);
// Cache the contact point (in world coordinates) to avoid
// recalculation if requested by the user.
dCopyVector3(contacts[0], c[0]);
dCopyVector3(contacts[1], c[1]);
}
void setBall2( dxJoint *joint, dxJoint::Info2 *info,
dVector3 anchor1, dVector3 anchor2,
dVector3 axis, dReal erp1 )
{
// anchor points in global coordinates with respect to body PORs.
dVector3 a1, a2;
int i, s = info->rowskip;
// get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
// [0 1 0] and [0 0 1], which makes everything much easier.
dVector3 q1, q2;
dPlaneSpace( axis, q1, q2 );
// set jacobian
for ( i = 0; i < 3; i++ ) info->J1l[i] = axis[i];
for ( i = 0; i < 3; i++ ) info->J1l[s+i] = q1[i];
for ( i = 0; i < 3; i++ ) info->J1l[2*s+i] = q2[i];
dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
dCalcVectorCross3( info->J1a, a1, axis );
dCalcVectorCross3( info->J1a + s, a1, q1 );
dCalcVectorCross3( info->J1a + 2*s, a1, q2 );
if ( joint->node[1].body )
{
for ( i = 0; i < 3; i++ ) info->J2l[i] = -axis[i];
for ( i = 0; i < 3; i++ ) info->J2l[s+i] = -q1[i];
for ( i = 0; i < 3; i++ ) info->J2l[2*s+i] = -q2[i];
dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
dReal *J2a = info->J2a;
dCalcVectorCross3( J2a, a2, axis );
dNegateVector3( J2a );
dReal *J2a_plus_s = J2a + s;
dCalcVectorCross3( J2a_plus_s, a2, q1 );
dNegateVector3( J2a_plus_s );
dReal *J2a_plus_2s = J2a_plus_s + s;
dCalcVectorCross3( J2a_plus_2s, a2, q2 );
dNegateVector3( J2a_plus_2s );
}
// set right hand side - measure error along (axis,q1,q2)
dReal k1 = info->fps * erp1;
dReal k = info->fps * info->erp;
for ( i = 0; i < 3; i++ ) a1[i] += joint->node[0].body->posr.pos[i];
if ( joint->node[1].body )
{
for ( i = 0; i < 3; i++ ) a2[i] += joint->node[1].body->posr.pos[i];
dVector3 a2_minus_a1;
dSubtractVectors3(a2_minus_a1, a2, a1);
info->c[0] = k1 * dCalcVectorDot3( axis, a2_minus_a1 );
info->c[1] = k * dCalcVectorDot3( q1, a2_minus_a1 );
info->c[2] = k * dCalcVectorDot3( q2, a2_minus_a1 );
}
else
{
dVector3 anchor2_minus_a1;
dSubtractVectors3(anchor2_minus_a1, anchor2, a1);
info->c[0] = k1 * dCalcVectorDot3( axis, anchor2_minus_a1 );
info->c[1] = k * dCalcVectorDot3( q1, anchor2_minus_a1 );
info->c[2] = k * dCalcVectorDot3( q2, anchor2_minus_a1 );
}
}
void
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
const int s1 = info->rowskip;
const int s2 = 2 * s1;
const dReal k = worldFPS * worldERP;
// ======================================================================
// The angular constraint
//
dVector3 ax1, ax2; // Global axes of rotation
getAxis(this, ax1, axis1);
getAxis2(this,ax2, axis2);
dVector3 uniPerp; // Axis perpendicular to axes of rotation
dCalcVectorCross3(uniPerp,ax1,ax2);
dNormalize3( uniPerp );
dCopyVector3( info->J1a , uniPerp );
if ( node[1].body )
{
dCopyNegatedVector3( info->J2a , uniPerp );
}
// Corrective velocity attempting to keep uni axes perpendicular
dReal val = dCalcVectorDot3( ax1, ax2 );
// Small angle approximation :
// theta = asin(val)
// theta is approximately val when val is near zero.
info->c[0] = -k * val;
// ==========================================================================
// Handle axes orthogonal to the prismatic
dVector3 an1, an2; // Global anchor positions
dVector3 axP, sep; // Prismatic axis and separation vector
getAnchor(this,an1,anchor1);
getAnchor2(this,an2,anchor2);
if (flags & dJOINT_REVERSE) {
getAxis2(this, axP, axisP1);
} else {
getAxis(this, axP, axisP1);
}
dSubtractVectors3(sep,an2,an1);
dVector3 p,q;
dPlaneSpace(axP,p,q);
dCopyVector3(( info->J1l ) + s1, p );
dCopyVector3(( info->J1l ) + s2, q );
// Make the anchors be body local
// Aliasing isn't a problem here.
dSubtractVectors3(an1,an1,node[0].body->posr.pos);
dCalcVectorCross3(( info->J1a ) + s1, an1, p );
dCalcVectorCross3(( info->J1a ) + s2, an1, q );
if (node[1].body) {
dCopyNegatedVector3(( info->J2l ) + s1, p );
dCopyNegatedVector3(( info->J2l ) + s2, q );
dSubtractVectors3(an2,an2,node[1].body->posr.pos);
dCalcVectorCross3(( info->J2a ) + s1, p, an2 );
dCalcVectorCross3(( info->J2a ) + s2, q, an2 );
}
info->c[1] = k * dCalcVectorDot3( p, sep );
info->c[2] = k * dCalcVectorDot3( q, sep );
// ==========================================================================
// Handle the limits/motors
int row = 3 + limot1.addLimot( this, worldFPS, info, 3, ax1, 1 );
row += limot2.addLimot( this, worldFPS, info, row, ax2, 1 );
if ( node[1].body || !(flags & dJOINT_REVERSE) )
limotP.addTwoPointLimot( this, worldFPS, info, row, axP, an1, an2 );
else
{
axP[0] = -axP[0];
axP[1] = -axP[1];
axP[2] = -axP[2];
limotP.addTwoPointLimot ( this, worldFPS, info, row, axP, an1, an2 );
}
}
void
dxJointSlider::getInfo2 ( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
int i, s = info->rowskip;
int s3 = 3 * s, s4 = 4 * s;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2, *R1, *R2;
dVector3 c;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
for ( i = 0; i < 3; i++ )
c[i] = pos2[i] - pos1[i];
}
else
{
pos2 = 0;
R2 = 0;
}
// 3 rows to make body rotations equal
setFixedOrientation ( this, worldFPS, worldERP, info, qrel, 0 );
// remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
dVector3 ax1; // joint axis in global coordinates (unit length)
dVector3 p, q; // plane space of ax1
dMultiply0_331 ( ax1, R1, axis1 );
dPlaneSpace ( ax1, p, q );
if ( node[1].body )
{
dVector3 tmp;
dCalcVectorCross3( tmp, c, p );
dScaleVector3r4( tmp, REAL( 0.5 ));
for ( i = 0; i < 3; i++ ) info->J1a[s3+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2a[s3+i] = tmp[i];
dCalcVectorCross3( tmp, c, q );
dScaleVector3r4( tmp, REAL( 0.5 ));
for ( i = 0; i < 3; i++ ) info->J1a[s4+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2a[s4+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2l[s3+i] = -p[i];
for ( i = 0; i < 3; i++ ) info->J2l[s4+i] = -q[i];
}
for ( i = 0; i < 3; i++ ) info->J1l[s3+i] = p[i];
for ( i = 0; i < 3; i++ ) info->J1l[s4+i] = q[i];
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
dReal k = worldFPS * worldERP;
if ( node[1].body )
{
dVector3 ofs; // offset point in global coordinates
dMultiply0_331 ( ofs, R2, offset );
for ( i = 0; i < 3; i++ ) c[i] += ofs[i];
info->c[3] = k * dCalcVectorDot3 ( p, c );
info->c[4] = k * dCalcVectorDot3 ( q, c );
}
else
{
dVector3 ofs; // offset point in global coordinates
for ( i = 0; i < 3; i++ ) ofs[i] = offset[i] - pos1[i];
info->c[3] = k * dCalcVectorDot3 ( p, ofs );
info->c[4] = k * dCalcVectorDot3 ( q, ofs );
if ( flags & dJOINT_REVERSE )
for ( i = 0; i < 3; ++i ) ax1[i] = -ax1[i];
}
// if the slider is powered, or has joint limits, add in the extra row
limot.addLimot ( this, worldFPS, info, 5, ax1, 0 );
}
void
dxJointPU::getInfo2( dxJoint::Info2 *info )
{
const int s0 = 0;
const int s1 = info->rowskip;
const int s2 = 2 * s1;
const dReal k = info->fps * info->erp;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2 = 0, *R1, *R2 = 0;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
}
dVector3 axP; // Axis of the prismatic joint in global frame
dMultiply0_331( axP, R1, axisP1 );
// distance between the body1 and the anchor2 in global frame
// Calculated in the same way as the offset
dVector3 dist;
dVector3 wanchor2 = {0,0,0};
if ( node[1].body )
{
dMultiply0_331( wanchor2, R2, anchor2 );
dist[0] = wanchor2[0] + pos2[0] - pos1[0];
dist[1] = wanchor2[1] + pos2[1] - pos1[1];
dist[2] = wanchor2[2] + pos2[2] - pos1[2];
}
else
{
if (flags & dJOINT_REVERSE )
{
// Invert the sign of dist
dist[0] = pos1[0] - anchor2[0];
dist[1] = pos1[1] - anchor2[1];
dist[2] = pos1[2] - anchor2[2];
}
else
{
dist[0] = anchor2[0] - pos1[0];
dist[1] = anchor2[1] - pos1[1];
dist[2] = anchor2[2] - pos1[2];
}
}
dVector3 q; // Temporary axis vector
// Will be used at 2 places with 2 different meaning
// ======================================================================
// Work on the angular part (i.e. row 0)
//
// The axis perpendicular to both axis1 and axis2 should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the rotoide axes should be equal. Thus the constraint equations are
// p*w1 - p*w2 = 0
// where p is a unit vector perpendicular to both axis1 and axis2
// and w1 and w2 are the angular velocity vectors of the two bodies.
dVector3 ax1, ax2;
getAxes( ax1, ax2 );
dReal val = dCalcVectorDot3( ax1, ax2 );
q[0] = ax2[0] - val * ax1[0];
q[1] = ax2[1] - val * ax1[1];
q[2] = ax2[2] - val * ax1[2];
dVector3 p;
dCalcVectorCross3( p, ax1, q );
dNormalize3( p );
// info->J1a[s0+i] = p[i];
dCopyVector3(( info->J1a ) + s0, p );
if ( node[1].body )
{
// info->J2a[s0+i] = -p[i];
dCopyNegatedVector3(( info->J2a ) + s0, p );
}
// compute the right hand side of the constraint equation. Set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
info->c[0] = k * - val;
// ==========================================================================
// Work on the linear part (i.e rows 1 and 2)
//
// We want: vel2 = vel1 + w1 x c ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the axisP is disregarded.
//
// p1 + R1 dist' = p2 + R2 anchor2'
// v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'
// v_p is speed of prismatic joint (i.e. elongation rate)
// Since the constraints are perpendicular to v_p we have:
// e1 dot v_p = 0 and e2 dot v_p = 0
// e1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
// e2 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
// ==
// e1 . v1 + e1 . w1 x dist = e1 . v2 + e1 . w2 x anchor2
// since a . (b x c) = - b . (a x c) = - (a x c) . b
// and a x b = - b x a
// e1 . v1 - e1 x dist . w1 - e1 . v2 - (- e1 x anchor2 . w2) = 0
// e1 . v1 + dist x e1 . w1 - e1 . v2 - anchor2 x e1 . w2 = 0
// Coeff for 1er line of: J1l => e1, J2l => -e1
// Coeff for 2er line of: J1l => e2, J2l => -ax2
// Coeff for 1er line of: J1a => dist x e1, J2a => - anchor2 x e1
// Coeff for 2er line of: J1a => dist x e2, J2a => - anchor2 x e2
// e1 and e2 are perpendicular to axP
// so e1 = ax1 and e2 = ax1 x axP
// N.B. ax2 is not always perpendicular to axP since it is attached to body 2
dCalcVectorCross3( q , ax1, axP );
dMultiply0_331( axP, R1, axisP1 );
dCalcVectorCross3(( info->J1a ) + s1, dist, ax1 );
dCalcVectorCross3(( info->J1a ) + s2, dist, q );
// info->J1l[s1+i] = ax[i];
dCopyVector3(( info->J1l ) + s1, ax1 );
// info->J1l[s2+i] = q[i];
dCopyVector3(( info->J1l ) + s2, q );
if ( node[1].body )
{
// Calculate anchor2 in world coordinate
// q x anchor2 instead of anchor2 x q since we want the negative value
dCalcVectorCross3(( info->J2a ) + s1, ax1, wanchor2 );
// The cross product is in reverse order since we want the negative value
dCalcVectorCross3(( info->J2a ) + s2, q, wanchor2 );
// info->J2l[s1+i] = -ax1[i];
dCopyNegatedVector3(( info->J2l ) + s1, ax1 );
// info->J2l[s2+i] = -ax1[i];
dCopyNegatedVector3(( info->J2l ) + s2, q );
}
// We want to make correction for motion not in the line of the axisP
// We calculate the displacement w.r.t. the anchor pt.
//
// compute the elements 1 and 2 of right hand side.
// We want to align the offset point (in body 2's frame) with the center of body 1.
// The position should be the same when we are not along the prismatic axis
dVector3 err;
dMultiply0_331( err, R1, anchor1 );
// err[i] = dist[i] - err[i];
dSubtractVectors3( err, dist, err );
info->c[1] = k * dCalcVectorDot3( ax1, err );
info->c[2] = k * dCalcVectorDot3( q, err );
int row = 3 + limot1.addLimot( this, info, 3, ax1, 1 );
row += limot2.addLimot( this, info, row, ax2, 1 );
if ( node[1].body || !(flags & dJOINT_REVERSE) )
limotP.addLimot( this, info, row, axP, 0 );
else
{
axP[0] = -axP[0];
axP[1] = -axP[1];
axP[2] = -axP[2];
limotP.addLimot ( this, info, row, axP, 0 );
}
}
// test one mesh triangle on intersection with capsule
void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule(
const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
uint8 flags)
{
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v1,m_vE1);
SUBTRACT(v0,v2,m_vE2);
dVector3 _minus_vE0;
SUBTRACT(v0,v1,_minus_vE0);
// calculate poly normal
dCalcVectorCross3(m_vN,m_vE1,_minus_vE0);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(m_vN))
{
return;
}
// create plane from triangle
dReal plDistance = -dCalcVectorDot3(v0,m_vN);
dVector4 plTrianglePlane;
CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance);
// calculate capsule distance to plane
dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition);
// Capsule must be over positive side of triangle
if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
{
// if not don't generate contacts
return;
}
dVector3 vPnt0;
SET (vPnt0,v0);
dVector3 vPnt1;
SET (vPnt1,v1);
dVector3 vPnt2;
SET (vPnt2,v2);
if (fDistanceCapsuleCenterToPlane < 0 )
{
SET (vPnt0,v0);
SET (vPnt1,v2);
SET (vPnt2,v1);
}
// do intersection test and find best separating axis
if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags))
{
// if not found do nothing
return;
}
// if best separation axis is not found
if (m_iBestAxis == 0 )
{
// this should not happen (we should already exit in that case)
dIASSERT(FALSE);
// do nothing
return;
}
// calculate caps centers in absolute space
dVector3 vCposTrans;
vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius;
vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius;
vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius;
dVector3 vCEdgePoint0;
vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCEdgePoint1;
vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// transform capsule edge points into triangle space
vCEdgePoint0[0] -= vPnt0[0];
vCEdgePoint0[1] -= vPnt0[1];
vCEdgePoint0[2] -= vPnt0[2];
vCEdgePoint1[0] -= vPnt0[0];
vCEdgePoint1[1] -= vPnt0[1];
vCEdgePoint1[2] -= vPnt0[2];
dVector4 plPlane;
dVector3 _minus_vN;
_minus_vN[0] = -m_vN[0];
_minus_vN[1] = -m_vN[1];
_minus_vN[2] = -m_vN[2];
// triangle plane
CONSTRUCTPLANE(plPlane,_minus_vN,0);
//plPlane = Plane4f( -m_vN, 0);
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
// plane with edge 0
dVector3 vTemp;
dCalcVectorCross3(vTemp,m_vN,m_vE0);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE1);
CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5)));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE2);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
return;
}
// return capsule edge points into absolute space
vCEdgePoint0[0] += vPnt0[0];
vCEdgePoint0[1] += vPnt0[1];
vCEdgePoint0[2] += vPnt0[2];
vCEdgePoint1[0] += vPnt0[0];
vCEdgePoint1[1] += vPnt0[1];
vCEdgePoint1[2] += vPnt0[2];
// calculate depths for both contact points
SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp);
dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp);
dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
// clamp depths to zero
if (fDepth0 < 0)
{
fDepth0 = 0.0f;
}
if (fDepth1 < 0 )
{
fDepth1 = 0.0f;
}
// Cached contacts's data
// contact 0
dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
m_gLocalContacts[m_ctContacts].fDepth = fDepth0;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
if (m_ctContacts < (m_iFlags & NUMC_MASK)) {
// contact 1
m_gLocalContacts[m_ctContacts].fDepth = fDepth1;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
}
}
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (RayGeom->type == dRayClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
RayCollider& Collider = pccColliderCache->_RayCollider;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
Collider.SetFirstContact(FirstContact != 0);
Collider.SetClosestHit(ClosestHit != 0);
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
/* Make Ray */
Ray WorldRay;
WorldRay.mOrig.x = Origin[0];
WorldRay.mOrig.y = Origin[1];
WorldRay.mOrig.z = Origin[2];
WorldRay.mDir.x = Direction[0];
WorldRay.mDir.y = Direction[1];
WorldRay.mDir.z = Direction[2];
/* Intersect */
Matrix4x4 amatrix;
int TriCount = 0;
if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
TriCount = pccColliderCache->Faces.GetNbFaces();
}
if (TriCount == 0) {
return 0;
}
const CollisionFace* Faces = pccColliderCache->Faces.GetFaces();
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++) {
if (TriMesh->RayCallback == null ||
TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
Faces[i].mU, Faces[i].mV)) {
const int& TriIndex = Faces[i].mFaceID;
if (!Callback(TriMesh, RayGeom, TriIndex)) {
continue;
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3 vu;
vu[0] = dv[1][0] - dv[0][0];
vu[1] = dv[1][1] - dv[0][1];
vu[2] = dv[1][2] - dv[0][2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = dv[2][0] - dv[0][0];
vv[1] = dv[2][1] - dv[0][1];
vv[2] = dv[2][2] - dv[0][2];
vv[3] = REAL(0.0);
dCalcVectorCross3(Contact->normal, vv, vu); // Reversed
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (dSafeNormalize3(Contact->normal))
{
// No sense to save on single type conversion in algorithm of this size.
// If there would be a custom typedef for distance type it could be used
// instead of dReal. However using float directly is the loss of abstraction
// and possible loss of precision in future.
/*float*/ dReal T = Faces[i].mDistance;
Contact->pos[0] = Origin[0] + (Direction[0] * T);
Contact->pos[1] = Origin[1] + (Direction[1] * T);
Contact->pos[2] = Origin[2] + (Direction[2] * T);
Contact->pos[3] = REAL(0.0);
Contact->depth = T;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
Contact->side1 = TriIndex;
Contact->side2 = -1;
OutTriCount++;
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
if (OutTriCount >= (Flags & NUMC_MASK)) {
break;
}
}
}
}
return OutTriCount;
}
void
dxJointScrew::getInfo2( dxJoint::Info2 *info )
{
// Added by OSRF
//
// Screw Constraint Overview
//
// make 5 constraint rows.
// given screw axis, first create two orthogonal axis p and q.
// row 1: linear constraint along p
// row 2: linear constraint along q
// row 3: screw constraint about user specified axis
// row 4: rotational constraint about p
// row 5: rotational constraint about q
// Added by OSRF
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
}
// constraint rows 1 to 3
{
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2, *R1, *R2;
dVector3 cgdiff;
cgdiff[0] = cgdiff[1] = cgdiff[2] = 0;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
for (int i = 0; i < 3; ++i )
{
// store distance between cg's in cgdiff
cgdiff[i] = pos2[i] - pos1[i];
}
}
else
{
pos2 = 0;
R2 = 0;
}
// compute error for screw due to drift
dReal lin_disp; // linear displacement
dReal lin_err; // linear displacement
{
// get linear disp for screw
// get axis1 in global coordinates
dVector3 ax1, q;
dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 );
if ( node[1].body )
{
// get body2 + offset point in global coordinates
dMultiply0_331 ( q, node[1].body->posr.R, offset );
//printf("debug offset q[%f %f %f] p0[%f %f %f] p1[%f %f %f] \t",
// q[0],q[1],q[2],
// node[0].body->posr.pos[0], node[0].body->posr.pos[1],
// node[0].body->posr.pos[2],
// node[1].body->posr.pos[0], node[1].body->posr.pos[1],
// node[1].body->posr.pos[2]);
for ( int ii = 0; ii < 3; ++ii )
q[ii] = node[0].body->posr.pos[ii] - q[ii] -
node[1].body->posr.pos[ii];
}
else
{
q[0] = node[0].body->posr.pos[0] - offset[0];
q[1] = node[0].body->posr.pos[1] - offset[1];
q[2] = node[0].body->posr.pos[2] - offset[2];
}
lin_disp = dCalcVectorDot3 ( ax1, q );
// linear error should be length scaled, BUT
if (dFabs(thread_pitch) > 1.0)
{
// constraint is written in length scale, so
// linear error is length scaled.
lin_err = -(lin_disp-cumulative_angle/thread_pitch);
}
else
{
// here the entire constraint equation, including lin_err
// is multiplied by thread_pitch for |thread_pitch| less than 1.0
// for added numerical stability,
lin_err = -(thread_pitch*lin_disp-cumulative_angle);
}
// printf("lin disp: %f lin err: %f\n", lin_disp, lin_err);
}
int s0 = 0 * info->rowskip;
int s1 = 1 * info->rowskip;
int s2 = 2 * info->rowskip;
// remaining two rows. we want: vel2 = vel1 + w1 x cgdiff ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
// ax1 is axis1 converted to body1 frame
dVector3 ax1;
dMultiply0_331 ( ax1, R1, axis1 );
// p and q are vectors perpendicular to ax1 in body1 frame
dVector3 p, q;
dPlaneSpace ( ax1, p, q );
// linear constraints for the hinge joint
// perpendicular to the sliding axis direction.
for (int i = 0; i < 3; ++i ) info->J1l[s0+i] = p[i];
for (int i = 0; i < 3; ++i ) info->J1l[s1+i] = q[i];
// if p and q do not pass through body CG's,
// we need to add angular constraints to balance out the forces
// from these linear constraints. See below:
// a1 and a2 are axis vectors in the body frame
// (whereas anchor1 and anchor2 are in world frame).
// anchor1 is the vector from CG to joint anchor in world frame.
dVector3 a1, a2;
dMultiply0_331( a1, R1, anchor1 );
// tmpp is a vector perpendicular to a1 and p in body frame,
// it is the direction of the angular constraint that will
// cancel out moment generated by linear constraint p
// if p does not pass through CG.
{
dVector3 tmpp;
dCalcVectorCross3(tmpp, p, a1);
for (int i = 0; i < 3; ++i ) info->J1a[s0+i] = -tmpp[i];
}
// tmpq is similar to tmpp, but for q.
{
dVector3 tmpq;
dCalcVectorCross3(tmpq, q, a1);
for (int i = 0; i < 3; ++i ) info->J1a[s1+i] = -tmpq[i];
}
// screw constraint:
// now constrain the sliding axis by rotation of the other body
if (dFabs(thread_pitch) > 1.0)
{
for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i];
for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i]/thread_pitch;
}
else
{
// here the entire constraint equation, including lin_err
// is multiplied by thread_pitch for |thread_pitch| less than 1.0
// for added numerical stability,
for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i]*thread_pitch;
for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i];
}
// repeat above for child body if one exists
if ( node[1].body )
{
// linear constraints for s0 and s1
for (int i = 0; i < 3; ++i ) info->J2l[s0+i] = -p[i];
for (int i = 0; i < 3; ++i ) info->J2l[s1+i] = -q[i];
// angular compensation if p and q do not pass through CG
dMultiply0_331( a2, R2, anchor2 );
dVector3 tmpp;
dCalcVectorCross3(tmpp, p, a2);
for (int i = 0; i < 3; ++i ) info->J2a[s0+i] = tmpp[i];
dVector3 tmpq;
dCalcVectorCross3(tmpq, q, a2);
for (int i = 0; i < 3; ++i ) info->J2a[s1+i] = tmpq[i];
// screw constraint:
// constrain the sliding axis by rotation of the other body
if (dFabs(thread_pitch) > 1.0)
{
for (int i = 0; i < 3; ++i ) info->J2a[s2+i] = ax1[i]/thread_pitch;
for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i];
}
else
{
// here the entire constraint equation, including lin_err
// is multiplied by thread_pitch for |thread_pitch| less than 1.0
// for added numerical stability,
for (int i = 0; i < 3; ++i ) info->J2a[s2+i] = ax1[i];
for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i]*thread_pitch;
}
}
// debug
// printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]);
// printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]);
// printf ("ax1 %f %f %f\n", ax1[0], ax1[1], ax1[2]);
// printf ("tmpp %f %f %f\n", tmpp[0], tmpp[1], tmpp[2]);
// printf ("p %f %f %f\n", p[0], p[1], p[2]);
// printf ("q %f %f %f\n", q[0], q[1], q[2]);
// printf ("J1a[s0] %f %f %f\n", info->J1a[s0+0], info->J1a[s0+1],
// info->J1a[s0+2]);
// printf ("J1a[s1] %f %f %f\n", info->J1a[s1+0], info->J1a[s1+1],
// info->J1a[s1+2]);
// info->J1a[s0+0] = 1;
// info->J1a[s0+1] = 0;
// info->J1a[s0+2] = 0;
// info->J1a[s1+0] = -1;
// info->J1a[s1+1] = 0;
// info->J1a[s1+2] = 0;
// printf("screw err lin[%f], ang[%f], diff[%f] tp[%f]\n",
// thread_pitch*lin_disp, cumulative_angle, lin_err, thread_pitch);
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
dReal k = info->fps * info->erp;
if ( node[1].body )
{
// dVector3 ofs; // offset point in global coordinates
// dMultiply0_331 ( ofs, R2, offset );
// for (int i = 0; i < 3; ++i ) cgdiff[i] += ofs[i];
// error between body anchors
dVector3 error12;
for (int i = 0; i < 3; ++i)
error12[i] = a2[i] + node[1].body->posr.pos[i] - a1[i] -
node[0].body->posr.pos[i];
// error in the p direction is error12 dot p
info->c[0] = k * (dCalcVectorDot3(error12, p));
// error in the q direction is error12 dot p
info->c[1] = k * (dCalcVectorDot3(error12, q));
// interpenetration error for screw constraint
info->c[2] = k * lin_err;
}
else
{
// debug
// printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]);
// printf ("anchor2 %f %f %f\n", anchor2[0], anchor2[1], anchor2[2]);
// printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]);
// printf ("p1 %f %f %f\n",
// node[0].body->posr.pos[0],
// node[0].body->posr.pos[1],
// node[0].body->posr.pos[2]);
// error of body's anchor
dVector3 error1;
for (int i = 0; i < 3; ++i) error1[i] = anchor2[i] - a1[i] -
node[0].body->posr.pos[i];
// printf ("error1 %f %f %f\n", error1[0], error1[1], error1[2]);
// error in the p direction is error1 dot p
info->c[0] = k * (dCalcVectorDot3(error1, p));
// error in the q direction
info->c[1] = k * (dCalcVectorDot3(error1, q));
// interpenetration error for screw constraint
info->c[2] = k * lin_err;
if ( flags & dJOINT_REVERSE )
for (int i = 0; i < 3; ++i ) ax1[i] = -ax1[i];
}
// uncommnet to enforce slider joint limit
// limot.addLimot ( this, info, 5, ax1, 0 );
}
// constraint rows 4 and 5
{
// set the two hinge rows. the screw axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
dVector3 p, q; // plane space vectors for ax1
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dPlaneSpace( ax1, p, q );
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
info->J1a[s4+0] = q[0];
info->J1a[s4+1] = q[1];
info->J1a[s4+2] = q[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
info->J2a[s4+0] = -q[0];
info->J2a[s4+1] = -q[1];
info->J2a[s4+2] = -q[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the screw back into alignment.
// if ax1,ax2 are the unit length screw axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
dVector3 ax2, b;
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
dCalcVectorCross3( b, ax1, ax2 );
dReal k = info->fps * info->erp;
info->c[3] = k * dCalcVectorDot3( b, p );
info->c[4] = k * dCalcVectorDot3( b, q );
// enforcing rotation joint limit
limot.addLimot( this, info, 5, ax1, 1 );
}
}
dReal dJointGetPUPositionRate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->node[0].body )
{
// We want to find the rate of change of the prismatic part of the joint
// We can find it by looking at the speed difference between body1 and the
// anchor point.
// r will be used to find the distance between body1 and the anchor point
dVector3 r;
dVector3 anchor2 = {0,0,0};
if ( joint->node[1].body )
{
// Find joint->anchor2 in global coordinates
dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
r[0] = ( joint->node[0].body->posr.pos[0] -
( anchor2[0] + joint->node[1].body->posr.pos[0] ) );
r[1] = ( joint->node[0].body->posr.pos[1] -
( anchor2[1] + joint->node[1].body->posr.pos[1] ) );
r[2] = ( joint->node[0].body->posr.pos[2] -
( anchor2[2] + joint->node[1].body->posr.pos[2] ) );
}
else
{
//N.B. When there is no body 2 the joint->anchor2 is already in
// global coordinates
// r = joint->node[0].body->posr.pos - joint->anchor2;
dSubtractVectors3( r, joint->node[0].body->posr.pos, joint->anchor2 );
}
// The body1 can have velocity coming from the rotation of
// the rotoide axis. We need to remove this.
// N.B. We do vel = r X w instead of vel = w x r to have vel negative
// since we want to remove it from the linear velocity of the body
dVector3 lvel1;
dCalcVectorCross3( lvel1, r, joint->node[0].body->avel );
// lvel1 += joint->node[0].body->lvel;
dAddVectors3( lvel1, lvel1, joint->node[0].body->lvel );
// Since we want rate of change along the prismatic axis
// get axisP1 in global coordinates and get the component
// along this axis only
dVector3 axP1;
dMultiply0_331( axP1, joint->node[0].body->posr.R, joint->axisP1 );
if ( joint->node[1].body )
{
// Find the contribution of the angular rotation to the linear speed
// N.B. We do vel = r X w instead of vel = w x r to have vel negative
// since we want to remove it from the linear velocity of the body
dVector3 lvel2;
dCalcVectorCross3( lvel2, anchor2, joint->node[1].body->avel );
// lvel1 -= lvel2 + joint->node[1].body->lvel;
dVector3 tmp;
dAddVectors3( tmp, lvel2, joint->node[1].body->lvel );
dSubtractVectors3( lvel1, lvel1, tmp );
return dCalcVectorDot3( axP1, lvel1 );
}
else
{
dReal rate = dCalcVectorDot3( axP1, lvel1 );
return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
}
}
return 0.0;
}
BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule(
const dVector3 &v0,
const dVector3 &v1,
const dVector3 &v2,
uint8 flags)
{
// calculate caps centers in absolute space
dVector3 vCp0;
vCp0[0] = m_vCapsulePosition[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp0[1] = m_vCapsulePosition[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp0[2] = m_vCapsulePosition[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCp1;
vCp1[0] = m_vCapsulePosition[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp1[1] = m_vCapsulePosition[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCp1[2] = m_vCapsulePosition[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// reset best axis
m_iBestAxis = 0;
// reset best depth
m_fBestDepth = -MAX_REAL;
// reset separating axis vector
dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
// Epsilon value for checking axis vector length
const dReal fEpsilon = 1e-6f;
// Translate triangle to Cc cord.
SUBTRACT(v0, m_vCapsulePosition, m_vV0);
SUBTRACT(v1, m_vCapsulePosition, m_vV1);
SUBTRACT(v2, m_vCapsulePosition, m_vV2);
// We begin to test for 19 separating axis now
// I wonder does it help if we employ the method like ISA-GJK???
// Or at least we should do experiment and find what axis will
// be most likely to be separating axis to check it first.
// Original
// axis m_vN
//vAxis = -m_vN;
vAxis[0] = - m_vN[0];
vAxis[1] = - m_vN[1];
vAxis[2] = - m_vN[2];
if (!_cldTestAxis(v0, v1, v2, vAxis, 1, TRUE))
{
return FALSE;
}
if (flags & dxTriMeshData::kEdge0)
{
// axis CxE0 - Edge 0
dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE0);
//vAxis = dCalcVectorCross3( m_vCapsuleAxis cross vE0 );
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge1)
{
// axis CxE1 - Edge 1
dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE1);
//vAxis = ( m_vCapsuleAxis cross m_vE1 );
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge2)
{
// axis CxE2 - Edge 2
//vAxis = ( m_vCapsuleAxis cross m_vE2 );
dCalcVectorCross3(vAxis,m_vCapsuleAxis,m_vE2);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge0)
{
// first capsule point
// axis ((Cp0-V0) x E0) x E0
_CalculateAxis(vCp0,v0,m_vE0,m_vE0,vAxis);
// vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 5)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge1)
{
// axis ((Cp0-V1) x E1) x E1
_CalculateAxis(vCp0,v1,m_vE1,m_vE1,vAxis);
//vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 6)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge2)
{
// axis ((Cp0-V2) x E2) x E2
_CalculateAxis(vCp0,v2,m_vE2,m_vE2,vAxis);
//vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 7)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge0)
{
// second capsule point
// axis ((Cp1-V0) x E0) x E0
_CalculateAxis(vCp1,v0,m_vE0,m_vE0,vAxis);
//vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 8)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge1)
{
// axis ((Cp1-V1) x E1) x E1
_CalculateAxis(vCp1,v1,m_vE1,m_vE1,vAxis);
//vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 9)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kEdge2)
{
// axis ((Cp1-V2) x E2) x E2
_CalculateAxis(vCp1,v2,m_vE2,m_vE2,vAxis);
//vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 10)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert0)
{
// first vertex on triangle
// axis ((V0-Cp0) x C) x C
_CalculateAxis(v0,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis);
//vAxis = ( ( v0-vCp0 ) cross m_vCapsuleAxis ) cross m_vCapsuleAxis;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert1)
{
// second vertex on triangle
// axis ((V1-Cp0) x C) x C
_CalculateAxis(v1,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis);
//vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert2)
{
// third vertex on triangle
// axis ((V2-Cp0) x C) x C
_CalculateAxis(v2,vCp0,m_vCapsuleAxis,m_vCapsuleAxis,vAxis);
//vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) {
return FALSE;
}
}
}
// Test as separating axes direction vectors between each triangle
// edge and each capsule's cap center
if (flags & dxTriMeshData::kVert0)
{
// first triangle vertex and first capsule point
//vAxis = v0 - vCp0;
SUBTRACT(v0,vCp0,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert1)
{
// second triangle vertex and first capsule point
//vAxis = v1 - vCp0;
SUBTRACT(v1,vCp0,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 15)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert2)
{
// third triangle vertex and first capsule point
//vAxis = v2 - vCp0;
SUBTRACT(v2,vCp0,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 16)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert0)
{
// first triangle vertex and second capsule point
//vAxis = v0 - vCp1;
SUBTRACT(v0,vCp1,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 17)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert1)
{
// second triangle vertex and second capsule point
//vAxis = v1 - vCp1;
SUBTRACT(v1,vCp1,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 18)) {
return FALSE;
}
}
}
if (flags & dxTriMeshData::kVert2)
{
// third triangle vertex and second capsule point
//vAxis = v2 - vCp1;
SUBTRACT(v2,vCp1,vAxis);
if (_length2OfVector3( vAxis ) > fEpsilon) {
if (!_cldTestAxis(v0, v1, v2, vAxis, 19)) {
return FALSE;
}
}
}
return TRUE;
}
int dxJointLimitMotor::addLimot( dxJoint *joint,
dxJoint::Info2 *info, int row,
const dVector3 ax1, int rotational )
{
int srow = row * info->rowskip;
// if the joint is powered, or has joint limits, add in the extra row
int powered = fmax > 0;
if ( powered || limit )
{
dReal *J1 = rotational ? info->J1a : info->J1l;
dReal *J2 = rotational ? info->J2a : info->J2l;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if ( joint->node[1].body )
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// halfway between the body centers. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
if ( !rotational && joint->node[1].body )
{
dVector3 c;
c[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] );
c[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] );
c[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] );
dCalcVectorCross3( ltd, c, ax1 );
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if ( limit && ( lostop == histop ) ) powered = 0;
if ( powered )
{
info->cfm[row] = normal_cfm;
if ( ! limit )
{
info->c[row] = vel;
info->lo[row] = -fmax;
info->hi[row] = fmax;
}
else
{
// the joint is at a limit, AND is being powered. if the joint is
// being powered into the limit then we apply the maximum motor force
// in that direction, because the motor is working against the
// immovable limit. if the joint is being powered away from the limit
// then we have problems because actually we need *two* lcp
// constraints to handle this case. so we fake it and apply some
// fraction of the maximum force. the fraction to use can be set as
// a fudge factor.
dReal fm = fmax;
if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
// if we're powering away from the limit, apply the fudge factor
if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
if ( rotational )
{
dBodyAddTorque( joint->node[0].body, -fm*ax1[0], -fm*ax1[1],
-fm*ax1[2] );
if ( joint->node[1].body )
dBodyAddTorque( joint->node[1].body, fm*ax1[0], fm*ax1[1], fm*ax1[2] );
}
else
{
dBodyAddForce( joint->node[0].body, -fm*ax1[0], -fm*ax1[1], -fm*ax1[2] );
if ( joint->node[1].body )
{
dBodyAddForce( joint->node[1].body, fm*ax1[0], fm*ax1[1], fm*ax1[2] );
// linear limot torque decoupling step: refer to above discussion
dBodyAddTorque( joint->node[0].body, -fm*ltd[0], -fm*ltd[1],
-fm*ltd[2] );
dBodyAddTorque( joint->node[1].body, -fm*ltd[0], -fm*ltd[1],
-fm*ltd[2] );
}
}
}
}
if ( limit )
{
dReal k = info->fps * stop_erp;
info->c[row] = -k * limit_err;
info->cfm[row] = stop_cfm;
if ( lostop == histop )
{
// limited low and high simultaneously
info->lo[row] = -dInfinity;
info->hi[row] = dInfinity;
}
else
{
if ( limit == 1 )
{
// low limit
info->lo[row] = 0;
info->hi[row] = dInfinity;
}
else
{
// high limit
info->lo[row] = -dInfinity;
info->hi[row] = 0;
}
// deal with bounce
if ( bounce > 0 )
{
// calculate joint velocity
dReal vel;
if ( rotational )
{
vel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
if ( joint->node[1].body )
vel -= dCalcVectorDot3( joint->node[1].body->avel, ax1 );
}
else
{
vel = dCalcVectorDot3( joint->node[0].body->lvel, ax1 );
if ( joint->node[1].body )
vel -= dCalcVectorDot3( joint->node[1].body->lvel, ax1 );
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if ( limit == 1 )
{
// low limit
if ( vel < 0 )
{
dReal newc = -bounce * vel;
if ( newc > info->c[row] ) info->c[row] = newc;
}
}
else
{
// high limit - all those computations are reversed
if ( vel > 0 )
{
dReal newc = -bounce * vel;
if ( newc < info->c[row] ) info->c[row] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
