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 }