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;
}
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
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
dxJointDBall::getInfo2( dxJoint::Info2 *info )
{
info->erp = erp;
info->cfm[0] = cfm;
dVector3 globalA1, globalA2;
dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
if (node[1].body)
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
else
dCopyVector3(globalA2, anchor2);
dVector3 q;
dSubtractVectors3(q, globalA1, globalA2);
#ifdef dSINGLE
const dReal MIN_LENGTH = REAL(1e-7);
#else
const dReal MIN_LENGTH = REAL(1e-12);
#endif
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// too small, let's choose an arbitrary direction
// heuristic: difference in velocities at anchors
dVector3 v1, v2;
dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
if (node[1].body)
dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
else
dSetZero(v2, 3);
dSubtractVectors3(q, v1, v2);
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// this direction is as good as any
q[0] = 1;
q[1] = 0;
q[2] = 0;
}
}
dNormalize3(q);
info->J1l[0] = q[0];
info->J1l[1] = q[1];
info->J1l[2] = q[2];
dVector3 relA1;
dBodyVectorToWorld(node[0].body,
anchor1[0], anchor1[1], anchor1[2],
relA1);
dMatrix3 a1m;
dSetZero(a1m, 12);
dSetCrossMatrixMinus(a1m, relA1, 4);
dMultiply1_331(info->J1a, a1m, q);
if (node[1].body) {
info->J2l[0] = -q[0];
info->J2l[1] = -q[1];
info->J2l[2] = -q[2];
dVector3 relA2;
dBodyVectorToWorld(node[1].body,
anchor2[0], anchor2[1], anchor2[2],
relA2);
dMatrix3 a2m;
dSetZero(a2m, 12);
dSetCrossMatrixPlus(a2m, relA2, 4);
dMultiply1_331(info->J2a, a2m, q);
}
const dReal k = info->fps * info->erp;
info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
}
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 );
}
}
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 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);
}
}
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 nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
void
dxJointPR::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
dReal k = worldFPS * worldERP;
dVector3 q; // plane space of axP and after that axR
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos2 = NULL, *R2 = NULL;
dReal *pos1 = node[0].body->posr.pos;
dReal *R1 = node[0].body->posr.R;
dxBody *body1 = node[1].body;
if ( body1 )
{
pos2 = body1->posr.pos;
R2 = body1->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 wanchor2 = {0, 0, 0}, dist;
if ( body1 )
{
// Calculate anchor2 in world coordinate
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) != 0 )
{
dSubtractVectors3(dist, pos1, anchor2); // Invert the value
}
else
{
dSubtractVectors3(dist, anchor2, pos1); // Invert the value
}
}
// ======================================================================
// Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered
// Set the two rotoide rows. The rotoide axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the rotoide 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 rotoide axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
dVector3 ax2;
dVector3 ax1;
dMultiply0_331( ax1, R1, axisR1 );
dCalcVectorCross3( q , ax1, axP );
dCopyVector3(J1 + GI2__JA_MIN, axP);
if ( body1 )
{
dCopyNegatedVector3(J2 + GI2__JA_MIN, axP);
}
dCopyVector3(J1 + rowskip + GI2__JA_MIN, q);
if ( body1 )
{
dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, q);
}
// Compute the right hand side of the constraint equation set. Relative
// body velocities along p and q to bring the rotoide back into alignment.
// ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
// 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.
if ( body1 )
{
dMultiply0_331( ax2, R2, axisR2 );
}
else
{
dCopyVector3(ax2, axisR2);
}
dVector3 b;
dCalcVectorCross3( b, ax1, ax2 );
pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3( b, axP );
pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( b, q );
// ==========================
// Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
// or 5 if rotoide and prismatic powered
// 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 prismatic axis is disregarded. for symmetry we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
// p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
// v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p
// v_p is speed of prismatic joint (i.e. elongation rate)
// Since the constraints are perpendicular to v_p we have:
// p dot v_p = 0 and q dot v_p = 0
// ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
// q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
// ==
// ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist
// since a . (b x c) = - b . (a x c) = - (a x c) . b
// and a x b = - b x a
// ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
// ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
// Coeff for 1er line of: J1l => ax1, J2l => -ax1
// Coeff for 2er line of: J1l => q, J2l => -q
// Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
// Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q
int currRowSkip = 2 * rowskip;
{
dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, ax1 );
dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, ax1 );
if ( body1 )
{
dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, ax1 );
// ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, ax2, wanchor2 ); // since ax1 == ax2
}
}
currRowSkip += rowskip;
{
dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q );
dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, dist, q );
if ( body1 )
{
dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, q);
// The cross product is in reverse order since we want the negative value
dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, wanchor2 );
}
}
// 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 2 and 3 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, offset );
dSubtractVectors3(err, dist, err);
int currPairSkip = 2 * pairskip;
{
pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( ax1, err );
}
currPairSkip += pairskip;
{
pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( q, err );
}
currRowSkip += rowskip; currPairSkip += pairskip;
if ( body1 || (flags & dJOINT_REVERSE) == 0 )
{
if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, 0 ))
{
currRowSkip += rowskip; currPairSkip += pairskip;
}
}
else
{
dVector3 rAxP;
dCopyNegatedVector3(rAxP, axP);
if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAxP, 0 ))
{
currRowSkip += rowskip; currPairSkip += pairskip;
}
}
limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
}
// Ray-Cylinder collider by Joseph Cooper (2011)
int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dRayClass );
dIASSERT( o2->type == dCylinderClass );
dIASSERT( (flags & NUMC_MASK) >= 1 );
dxRay* ray = (dxRay*)( o1 );
dxCylinder* cyl = (dxCylinder*)( o2 );
// Fill in contact information.
contact->g1 = ray;
contact->g2 = cyl;
contact->side1 = -1;
contact->side2 = -1;
const dReal half_length = cyl->lz * REAL( 0.5 );
/* Possible collision cases:
* Ray origin between/outside caps
* Ray origin within/outside radius
* Ray direction left/right/perpendicular
* Ray direction parallel/perpendicular/other
*
* Ray origin cases (ignoring origin on surface)
*
* A B
* /-\-----------\
* C ( ) D )
* \_/___________/
*
* Cases A and D can collide with caps or cylinder
* Case C can only collide with the caps
* Case B can only collide with the cylinder
* Case D will produce inverted normals
* If the ray is perpendicular, only check the cylinder
* If the ray is parallel to cylinder axis,
* we can only check caps
* If the ray points right,
* Case A,C Check left cap
* Case D Check right cap
* If the ray points left
* Case A,C Check right cap
* Case D Check left cap
* Case B, check only first possible cylinder collision
* Case D, check only second possible cylinder collision
*/
// Find the ray in the cylinder coordinate frame:
dVector3 tmp;
dVector3 pos; // Ray origin in cylinder frame
dVector3 dir; // Ray direction in cylinder frame
// Translate ray start by inverse cyl
dSubtractVectors3(tmp,ray->final_posr->pos,cyl->final_posr->pos);
// Rotate ray start by inverse cyl
dMultiply1_331(pos,cyl->final_posr->R,tmp);
// Get the ray's direction
tmp[0] = ray->final_posr->R[2];
tmp[1] = ray->final_posr->R[6];
tmp[2] = ray->final_posr->R[10];
// Rotate the ray direction by inverse cyl
dMultiply1_331(dir,cyl->final_posr->R,tmp);
// Is the ray origin inside of the (extended) cylinder?
dReal r2 = cyl->radius*cyl->radius;
dReal C = pos[0]*pos[0] + pos[1]*pos[1] - r2;
// Find the different cases
// Is ray parallel to the cylinder length?
int parallel = (dir[0]==0 && dir[1]==0);
// Is ray perpendicular to the cylinder length?
int perpendicular = (dir[2]==0);
// Is ray origin within the radius of the caps?
int inRadius = (C<=0);
// Is ray origin between the top and bottom caps?
int inCaps = (dFabs(pos[2])<=half_length);
int checkCaps = (!perpendicular && (!inCaps || inRadius));
int checkCyl = (!parallel && (!inRadius || inCaps));
int flipNormals = (inCaps&&inRadius);
dReal tt=-dInfinity; // Depth to intersection
dVector3 tmpNorm = {dNaN, dNaN, dNaN}; // ensure we don't leak garbage
if (checkCaps) {
// Make it so we only need to check one cap
int flipDir = 0;
// Wish c had logical xor...
if ((dir[2]<0 && flipNormals) || (dir[2]>0 && !flipNormals)) {
flipDir = 1;
dir[2]=-dir[2];
pos[2]=-pos[2];
}
// The cap is half the cylinder's length
// from the cylinder's origin
// We only checkCaps if dir[2]!=0
tt = (half_length-pos[2])/dir[2];
if (tt>=0 && tt<=ray->length) {
tmp[0] = pos[0] + tt*dir[0];
tmp[1] = pos[1] + tt*dir[1];
// Ensure collision point is within cap circle
if (tmp[0]*tmp[0] + tmp[1]*tmp[1] <= r2) {
// Successful collision
tmp[2] = (flipDir)?-half_length:half_length;
tmpNorm[0]=0;
tmpNorm[1]=0;
tmpNorm[2]=(flipDir!=flipNormals)?-1:1;
checkCyl = 0; // Short circuit cylinder check
} else {
// Ray hits cap plane outside of cap circle
tt=-dInfinity; // No collision yet
}
} else {
// The cap plane is beyond (or behind) the ray length
tt=-dInfinity; // No collision yet
}
if (flipDir) {
// Flip back
dir[2]=-dir[2];
pos[2]=-pos[2];
}
}
if (checkCyl) {
// Compute quadratic formula for parametric ray equation
dReal A = dir[0]*dir[0] + dir[1]*dir[1];
dReal B = 2*(pos[0]*dir[0] + pos[1]*dir[1]);
// Already computed C
dReal k = B*B - 4*A*C;
// Check collision with infinite cylinder
// k<0 means the ray passes outside the cylinder
// k==0 means ray is tangent to cylinder (or parallel)
//
// Our quadratic formula: tt = (-B +- sqrt(k))/(2*A)
//
// A must be positive (otherwise we wouldn't be checking
// cylinder because ray is parallel)
// if (k<0) ray doesn't collide with sphere
// if (B > sqrt(k)) then both times are negative
// -- don't calculate
// if (B<-sqrt(k)) then both times are positive (Case A or B)
// -- only calculate first, if first isn't valid
// -- second can't be without first going through a cap
// otherwise (fabs(B)<=sqrt(k)) then C<=0 (ray-origin inside/on cylinder)
// -- only calculate second collision
if (k>=0 && (B<0 || B*B<=k)) {
k = dSqrt(k);
A = dRecip(2*A);
if (dFabs(B)<=k) {
tt = (-B + k)*A; // Second solution
// If ray origin is on surface and pointed out, we
// can get a tt=0 solution...
} else {
tt = (-B - k)*A; // First solution
}
if (tt<=ray->length) {
tmp[2] = pos[2] + tt*dir[2];
if (dFabs(tmp[2])<=half_length) {
// Valid solution
tmp[0] = pos[0] + tt*dir[0];
tmp[1] = pos[1] + tt*dir[1];
tmpNorm[0] = tmp[0]/cyl->radius;
tmpNorm[1] = tmp[1]/cyl->radius;
tmpNorm[2] = 0;
if (flipNormals) {
// Ray origin was inside cylinder
tmpNorm[0] = -tmpNorm[0];
tmpNorm[1] = -tmpNorm[1];
}
} else {
// Ray hits cylinder outside of caps
tt=-dInfinity;
}
} else {
// Ray doesn't reach the cylinder
tt=-dInfinity;
}
}
}
if (tt>0) {
contact->depth = tt;
// Transform the point back to world coordinates
tmpNorm[3]=0;
tmp[3] = 0;
dMultiply0_331(contact->normal,cyl->final_posr->R,tmpNorm);
dMultiply0_331(contact->pos,cyl->final_posr->R,tmp);
contact->pos[0]+=cyl->final_posr->pos[0];
contact->pos[1]+=cyl->final_posr->pos[1];
contact->pos[2]+=cyl->final_posr->pos[2];
return 1;
}
// No contact with anything.
return 0;
}
void dxPlanarJoint::getInfo2(dReal worldFPS, dReal worldERP, const Info2Descr* info)
{
/* ====================================================================
* This joint restricts 2 rotational and 1 translational DOF.
*
* The 2 rotational constraints are essentially the same as in dxJointPiston, so the implementation here is mostly
* just a copy of them.
*
* The translational 1DOF constraint has not yet been implemented for any joint in ODE, so we'll talk more about it.
* It is similar to the constraints in a slider joint, but in this case we only want body2 to move the plane, and
* body1 anchor has no use in this case, too.
*
* We basically want to express the plane in body2 local coordinates (because it should move along with body2), and
* check, if body1 origin lies in the plane.
*
* Let's say n' is the plane normal in body2 coordinates and a' is the anchor point in body2 coordinates
* (with n, a, being the corresponding global variables).
* Further, let's call the global position of body1 p1, and similarly p2 for body2.
* Last, let's call body2 rotation matrix R2 (so that n = R2*n' and a = R2*a' + p2).
*
* The plane can then be expressed in global coordinates as:
* (n^ . x) - (n^ . a) = 0,
* where x is in global coordinates and ^ denotes vector/matrix transpose.
*
* Rewriting the equation using body2 local coordinates and setting x=p1, we get the constraint:
* (R2*n')^ * p1 - (R2*n')^ * (R2*a' + p2) = 0
* ...
* (n'^ * R2^ * p1) - (n'^ * R2^ * p2) - (n'^ * a') = 0
* (n^ * p1) - (n^ * p2) - (n'^ * a') = 0
*
* The part left to "=" will be the "c" on the RHS of the Jacobian equation (because it expresses
* the distance of p1 from the plane).
*
* Next, we need to take time derivative of the constraint to get the J1 and J2 parts.
* v1, v2, w1 and w2 denote the linear and angular velocities od body1 and body2.
* [a]x denotes the skew-symmetric cross-product matrix of vector a.
*
* d/dt [(n'^ * R2^ * p1) - (n'^ * R2^ * p2) - (n'^ * a') = 0] (n', a' are constant in time)
* n'^ * ((d/dt[R2^] * p1) + (R2^ * v1)) - n'^ * ((d/dt[R2^] * p2) + (R2^ * v2)) = 0
* n'^ * ((([w2]x R2)^ * p1) + (R2^ * v1)) - n'^ * ((([w2]x R2)^ * p2) + (R2^ * v2)) = 0
* ...
* n^*v1 - n^*v2 + [n]x (p1 - p2) . w2 = 0
* -n^*v1 + n^*v2 + [n]x (p2 - p1) . w2 = 0
*
* Thus we see that
* J1l = -n
* J2l = n
* J1a = 0
* J2a = [n]x (p2 - p1)
* c = eps * ( (n^ * p1) - (n^ * a) )
*
*/
const int row0Index = 0;
const int row1Index = info->rowskip;
const int row2Index = 2 * row1Index;
const dReal eps = worldFPS * worldERP;
dVector3 globalAxis1;
dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
dVector3 globalAxis2;
dVector3 globalAnchor;
if (node[1].body) {
dBodyVectorToWorld(node[1].body, axis2[0], axis2[1], axis2[2], globalAxis2);
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalAnchor);
} else {
dCopyVector3(globalAxis2, axis2);
dCopyVector3(globalAnchor, anchor2);
}
///////////////////////////////////////////////////////
// Angular velocity constraints (2 rows)
//
// Refer to the piston joint source code for details.
///////////////////////////////////////////////////////
// Find the 2 direction vectors of the plane.
dVector3 p, q;
dPlaneSpace ( globalAxis2, p, q );
// LHS 0
dCopyNegatedVector3 (info->J1a + row0Index, p );
dCopyNegatedVector3 (info->J1a + row1Index, q );
// LHS 1
dCopyVector3 (info->J2a + row0Index, p );
dCopyVector3 (info->J2a + row1Index, q );
// RHS 0 & 1 (absolute errors of the axis direction computed by dot product of the desired and actual axis)
dVector3 b;
dCalcVectorCross3( b, globalAxis2, globalAxis1 );
info->c[0] = eps * dCalcVectorDot3 ( p, b );
info->c[1] = eps * dCalcVectorDot3 ( q, b );
//////////////////////////////////////////////////////////////////////////////////////////
// Linear velocity constraint (1 row, removes 1 translational DOF along the plane normal)
//
// Only body1 should be restricted by the plane, which is moved along with body2.
//////////////////////////////////////////////////////////////////////////////////////////
dVector3 body1Center;
dCopyVector3(body1Center, node[0].body->posr.pos);
dVector3 body2Center = {0, 0, 0};
if (node[1].body) {
dCopyVector3(body2Center, node[1].body->posr.pos);
}
// LHS 2
dCopyNegatedVector3(info->J1l + row2Index, globalAxis2);
dCopyVector3( info->J2l + row2Index, globalAxis2);
dVector3 body2_body1;
dSubtractVectors3(body2_body1, body2Center, body1Center);
dCalcVectorCross3(info->J2a + row2Index, globalAxis2, body2_body1);
// RHS 2 (distance of body1 center from the plane)
info->c[2] = eps * (dCalcVectorDot3(globalAxis2, body1Center) - dCalcVectorDot3(globalAxis2, globalAnchor));
}
/*
* dMassSetTrimesh, implementation by Gero Mueller.
* Based on Brian Mirtich, "Fast and Accurate Computation of
* Polyhedral Mass Properties," journal of graphics tools, volume 1,
* number 2, 1996.
*/
void dMassSetTrimesh( dMass *m, dReal density, dGeomID g )
{
dAASSERT (m);
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dMassSetZero (m);
#if dTRIMESH_ENABLED
dxTriMesh *TriMesh = (dxTriMesh *)g;
unsigned int triangles = FetchTriangleCount( TriMesh );
dReal nx, ny, nz;
unsigned int i, A, B, C;
// face integrals
dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
// projection integrals
dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
dReal T0 = 0;
dReal T1[3] = {0., 0., 0.};
dReal T2[3] = {0., 0., 0.};
dReal TP[3] = {0., 0., 0.};
for( i = 0; i < triangles; i++ )
{
dVector3 v[3];
FetchTransformedTriangle( TriMesh, i, v);
dVector3 n, a, b;
dSubtractVectors3( a, v[1], v[0] );
dSubtractVectors3( b, v[2], v[0] );
dCalcVectorCross3( n, b, a );
nx = fabs(n[0]);
ny = fabs(n[1]);
nz = fabs(n[2]);
if( nx > ny && nx > nz )
C = 0;
else
C = (ny > nz) ? 1 : 2;
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (n[C] != REAL(0.0))
{
A = (C + 1) % 3;
B = (A + 1) % 3;
// calculate face integrals
{
dReal w;
dReal k1, k2, k3, k4;
//compProjectionIntegrals(f);
{
dReal a0=0, a1=0, da;
dReal b0=0, b1=0, db;
dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
dReal a1_2, a1_3, b1_2, b1_3;
dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
dReal Cab, Kab, Caab, Kaab, Cabb, Kabb;
P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
for( int j = 0; j < 3; j++)
{
switch(j)
{
case 0:
a0 = v[0][A];
b0 = v[0][B];
a1 = v[1][A];
b1 = v[1][B];
break;
case 1:
a0 = v[1][A];
b0 = v[1][B];
a1 = v[2][A];
b1 = v[2][B];
break;
case 2:
a0 = v[2][A];
b0 = v[2][B];
a1 = v[0][A];
b1 = v[0][B];
break;
}
da = a1 - a0;
db = b1 - b0;
a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
a1_2 = a1 * a1; a1_3 = a1_2 * a1;
b1_2 = b1 * b1; b1_3 = b1_2 * b1;
C1 = a1 + a0;
Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
P1 += db*C1;
Pa += db*Ca;
Paa += db*Caa;
Paaa += db*Caaa;
Pb += da*Cb;
Pbb += da*Cbb;
Pbbb += da*Cbbb;
Pab += db*(b1*Cab + b0*Kab);
Paab += db*(b1*Caab + b0*Kaab);
Pabb += da*(a1*Cabb + a0*Kabb);
}
P1 /= 2.0;
Pa /= 6.0;
Paa /= 12.0;
Paaa /= 20.0;
Pb /= -6.0;
Pbb /= -12.0;
Pbbb /= -20.0;
Pab /= 24.0;
Paab /= 60.0;
Pabb /= -60.0;
}
w = - dCalcVectorDot3(n, v[0]);
k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
Fa = k1 * Pa;
Fb = k1 * Pb;
Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
Faa = k1 * Paa;
Fbb = k1 * Pbb;
Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb +
w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
Faaa = k1 * Paaa;
Fbbb = k1 * Pbbb;
Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
+ 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
+ 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
+ w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
Faab = k1 * Paab;
Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
+ w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
}
T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));
T1[A] += n[A] * Faa;
T1[B] += n[B] * Fbb;
T1[C] += n[C] * Fcc;
T2[A] += n[A] * Faaa;
T2[B] += n[B] * Fbbb;
T2[C] += n[C] * Fccc;
TP[A] += n[A] * Faab;
TP[B] += n[B] * Fbbc;
TP[C] += n[C] * Fcca;
}
}
T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
m->mass = density * T0;
m->_I(0,0) = density * (T2[1] + T2[2]);
m->_I(1,1) = density * (T2[2] + T2[0]);
m->_I(2,2) = density * (T2[0] + T2[1]);
m->_I(0,1) = - density * TP[0];
m->_I(1,0) = - density * TP[0];
m->_I(2,1) = - density * TP[1];
m->_I(1,2) = - density * TP[1];
m->_I(2,0) = - density * TP[2];
m->_I(0,2) = - density * TP[2];
// Added to address SF bug 1729095
dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 );
# ifndef dNODEBUG
dMassCheck (m);
# endif
#endif // dTRIMESH_ENABLED
}
