static int edgeIntersectsRect (dVector3 v1, dVector3 v2, dVector3 p1, dVector3 p2, dVector3 p3) { int k; dVector3 u1,u2,n,tmp; for (k=0; k<3; k++) u1[k] = p3[k]-p1[k]; for (k=0; k<3; k++) u2[k] = p2[k]-p1[k]; dReal d1 = dSqrt(dCalcVectorDot3(u1,u1)); dReal d2 = dSqrt(dCalcVectorDot3(u2,u2)); dNormalize3 (u1); dNormalize3 (u2); if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2"); dCalcVectorCross3(n,u1,u2); for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k]; dReal d = -dCalcVectorDot3(n,p1); if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1"); if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2"); if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3"); dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp); for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]); if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp"); if (alpha < 0) return 0; if (alpha > 1) return 0; for (k=0; k<3; k++) tmp[k] -= p1[k]; dReal a1 = dCalcVectorDot3(u1,tmp); dReal a2 = dCalcVectorDot3(u2,tmp); if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0; return 1; }
void makeRandomRotation (dMatrix3 R) { dReal *u1 = R, *u2=R+4, *u3=R+8; dMakeRandomVector (u1,3,1.0); dNormalize3 (u1); dMakeRandomVector (u2,3,1.0); dReal d = dCalcVectorDot3(u1,u2); u2[0] -= d*u1[0]; u2[1] -= d*u1[1]; u2[2] -= d*u1[2]; dNormalize3(u2); dCalcVectorCross3(u3,u1,u2); }
void dxJointHinge2::makeW1andW2() { if ( node[1].body ) { // get axis 1 and 2 in global coords dVector3 ax1, ax2, w; dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); dMultiply0_331( ax2, node[1].body->posr.R, axis2 ); // don't do anything if the axis1 or axis2 vectors are zero or the same if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) || ( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) || ( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return; // modify axis 1 so it's perpendicular to axis 2 dReal k = dCalcVectorDot3( ax2, ax1 ); for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i]; dNormalize3( ax1 ); // make w1 = modified axis1, w2 = axis2 x (modified axis1) dCalcVectorCross3( w, ax2, ax1 ); dMultiply1_331( w1, node[1].body->posr.R, ax1 ); dMultiply1_331( w2, node[1].body->posr.R, w ); } }
//plane plane bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4], dVector3 LinePos,dVector3 LineDir) { // If Cross(N0,N1) is zero, then either planes are parallel and separated // or the same plane. In both cases, 'false' is returned. Otherwise, // the intersection line is // // L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1 // // for some coefficients c0 and c1 and for t any real number (the line // parameter). Taking dot products with the normals, // // d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1) // d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1) // // which are two equations in two unknowns. The solution is // // c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det // c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det // // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2. /* Real fN00 = rkPlane0.Normal().SquaredLength(); Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal()); Real fN11 = rkPlane1.Normal().SquaredLength(); Real fDet = fN00*fN11 - fN01*fN01; if ( Math::FAbs(fDet) < gs_fEpsilon ) return false; Real fInvDet = 1.0f/fDet; Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet; Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet; rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal()); rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal(); return true; */ dReal fN00 = dLENGTHSQUARED(Plane0); dReal fN01 = dDOT(Plane0,Plane1); dReal fN11 = dLENGTHSQUARED(Plane1); dReal fDet = fN00*fN11 - fN01*fN01; if ( fabs(fDet) < fEPSILON) return false; dReal fInvDet = 1.0f/fDet; dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet; dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet; dCROSS(LineDir,=,Plane0,Plane1); dNormalize3(LineDir); dVector3 Temp0,Temp1; dOPC(Temp0,*,Plane0,fC0); dOPC(Temp1,*,Plane1,fC1); dOP(LinePos,+,Temp0,Temp1); return true; }
void dxJointHinge2::makeV1andV2() { if ( node[0].body ) { // get axis 1 and 2 in global coords dVector3 ax1, ax2, v; dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); dMultiply0_331( ax2, node[1].body->posr.R, axis2 ); // don't do anything if the axis1 or axis2 vectors are zero or the same if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) || (_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) || (_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2]))) return; // modify axis 2 so it's perpendicular to axis 1 dReal k = dCalcVectorDot3( ax1, ax2 ); for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i]; dNormalize3( ax2 ); // make v1 = modified axis2, v2 = axis1 x (modified axis2) dCalcVectorCross3( v, ax1, ax2 ); dMultiply1_331( v1, node[0].body->posr.R, ax2 ); dMultiply1_331( v2, node[0].body->posr.R, v ); } }
void setAxes( dxJoint *j, dReal x, dReal y, dReal z, dVector3 axis1, dVector3 axis2 ) { if ( j->node[0].body ) { dReal q[4]; q[0] = x; q[1] = y; q[2] = z; q[3] = 0; dNormalize3( q ); if ( axis1 ) { dMultiply1_331( axis1, j->node[0].body->posr.R, q ); axis1[3] = 0; } if ( axis2 ) { if ( j->node[1].body ) { dMultiply1_331( axis2, j->node[1].body->posr.R, q ); } else { axis2[0] = x; axis2[1] = y; axis2[2] = z; } axis2[3] = 0; } } }
void fixFrictionVector( dBodyID b1, dBodyID b2, dContact& contact ) { dBodyGetPointVel(b1, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], contact.fdir1); dVector3 fdir1_b2; if (b2) { dBodyGetPointVel(b2, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], fdir1_b2); contact.fdir1[0] -= fdir1_b2[0]; contact.fdir1[1] -= fdir1_b2[1]; contact.fdir1[2] -= fdir1_b2[2]; } // at this point, contact[i].fdir1 is the relative tangent velocity of the two bodies. dCROSS(contact.fdir1, =, contact.fdir1, contact.geom.normal); // now, contact[i].fdir1 is perpendicular to both the normal and // the relative tangent velocity. double length = sqrt(contact.fdir1[0] * contact.fdir1[0] + contact.fdir1[1] * contact.fdir1[1] + contact.fdir1[2] * contact.fdir1[2]); if (length > 1e-12) { // we only use our calculated direction if it has enough precision contact.fdir1[0] /= length; contact.fdir1[1] /= length; contact.fdir1[2] /= length; dNormalize3(contact.fdir1); contact.surface.mode |= dContactFDir1; } }
void dJointSetGearboxAxis2( dJointID j, dReal x, dReal y, dReal z ) { dxJointGearbox* joint = static_cast<dxJointGearbox*>(j); dUASSERT( joint, "bad joint argument" ); dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2); dNormalize3(joint->axis2); }
int test_sphere_point_depth() { int j; dVector3 p,q; dMatrix3 R; dReal r,d; dSimpleSpace space(0); dGeomID sphere = dCreateSphere (0,1); dSpaceAdd (space,sphere); // ********** make a random sphere of radius r at position p r = dRandReal()+0.1; dGeomSphereSetRadius (sphere,r); dMakeRandomVector (p,3,1.0); dGeomSetPosition (sphere,p[0],p[1],p[2]); dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, dRandReal()*2-1,dRandReal()*10-5); dGeomSetRotation (sphere,R); // ********** test center point has depth r if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED(); // ********** test point on surface has depth 0 for (j=0; j<3; j++) q[j] = dRandReal()-0.5; dNormalize3 (q); for (j=0; j<3; j++) q[j] = q[j]*r + p[j]; if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED(); // ********** test point at random depth d = (dRandReal()*2-1) * r; for (j=0; j<3; j++) q[j] = dRandReal()-0.5; dNormalize3 (q); for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j]; if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED(); PASSED(); }
// compute the 3 axes in global coordinates void dxJointAMotor::computeGlobalAxes( dVector3 ax[3] ) { if ( mode == dAMotorEuler ) { // special handling for euler mode dMultiply0_331( ax[0], node[0].body->posr.R, axis[0] ); if ( node[1].body ) { dMultiply0_331( ax[2], node[1].body->posr.R, axis[2] ); } else { ax[2][0] = axis[2][0]; ax[2][1] = axis[2][1]; ax[2][2] = axis[2][2]; } dCalcVectorCross3( ax[1], ax[2], ax[0] ); dNormalize3( ax[1] ); } else { for ( int i = 0; i < num; i++ ) { if ( rel[i] == 1 ) { // relative to b1 dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] ); } else if ( rel[i] == 2 ) { // relative to b2 if ( node[1].body ) // jds: don't assert, just ignore { dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] ); } else { // global - just copy it ax[i][0] = axis[i][0]; ax[i][1] = axis[i][1]; ax[i][2] = axis[i][2]; } } else { // global - just copy it ax[i][0] = axis[i][0]; ax[i][1] = axis[i][1]; ax[i][2] = axis[i][2]; } } } }
void dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info ) { // get information we need to set the hinge row dReal s, c; dVector3 q; const dxJointHinge2 *joint = this; dVector3 ax1, ax2; joint->getAxisInfo( ax1, ax2, q, s, c ); dNormalize3( q ); // @@@ quicker: divide q by s ? // set the three ball-and-socket rows (aligned to the suspension axis ax1) setBall2( this, worldFPS, worldERP, info, anchor1, anchor2, ax1, susp_erp ); // set the hinge row int s3 = 3 * info->rowskip; info->J1a[s3+0] = q[0]; info->J1a[s3+1] = q[1]; info->J1a[s3+2] = q[2]; if ( joint->node[1].body ) { info->J2a[s3+0] = -q[0]; info->J2a[s3+1] = -q[1]; info->J2a[s3+2] = -q[2]; } // compute the right hand side for the constrained rotational DOF. // axis 1 and axis 2 are separated by an angle `theta'. the desired // separation angle is theta0. sin(theta0) and cos(theta0) are recorded // in the joint structure. the correcting angular velocity is: // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize // = (erp*fps) * (theta0-theta) // (theta0-theta) can be computed using the following small-angle-difference // approximation: // theta0-theta ~= tan(theta0-theta) // = sin(theta0-theta)/cos(theta0-theta) // = (c*s0 - s*c0) / (c*c0 + s*s0) // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 // where c = cos(theta), s = sin(theta) // c0 = cos(theta0), s0 = sin(theta0) dReal k = worldFPS * worldERP; info->c[3] = k * ( c0 * s - joint->s0 * c ); // if the axis1 hinge is powered, or has joint limits, add in more stuff int row = 4 + limot1.addLimot( this, worldFPS, info, 4, ax1, 1 ); // if the axis2 hinge is powered, add in more stuff limot2.addLimot( this, worldFPS, info, row, ax2, 1 ); // set parameter for the suspension info->cfm[0] = susp_cfm; }
void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z ) { dxJointAMotor* joint = ( dxJointAMotor* )j; dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 ); checktype( joint, AMotor ); dUASSERT( !( !joint->node[1].body && ( joint->flags & dJOINT_REVERSE ) && rel == 1 ), "no first body, can't set axis rel=1" ); dUASSERT( !( !joint->node[1].body && !( joint->flags & dJOINT_REVERSE ) && rel == 2 ), "no second body, can't set axis rel=2" ); if ( anum < 0 ) anum = 0; if ( anum > 2 ) anum = 2; // adjust rel to match the internal body order if ( !joint->node[1].body && rel == 2 ) rel = 1; joint->rel[anum] = rel; // x,y,z is always in global coordinates regardless of rel, so we may have // to convert it to be relative to a body dVector3 r; r[0] = x; r[1] = y; r[2] = z; r[3] = 0; if ( rel > 0 ) { if ( rel == 1 ) { dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r ); } else { // don't assert; handle the case of attachment to a bodiless geom if ( joint->node[1].body ) // jds { dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r ); } else { joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3]; } } } else { joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; joint->axis[anum][2] = r[2]; } dNormalize3( joint->axis[anum] ); if ( joint->mode == dAMotorEuler ) joint->setEulerReferenceVectors(); }
void dJointPlanarSetPlaneNormal(dJointID joint, dVector3 planeNormal) { dUASSERT( joint, "bad joint argument" ); checktype( joint, Plane2D ); dxPlanarJoint* planarJoint = (dxPlanarJoint*) joint; dUASSERT(dLENGTHSQUARED(planeNormal) > 0.000001, "plane normal cannot have zero length"); dCopyVector3(planarJoint->planeNormal, planeNormal); dNormalize3(planarJoint->planeNormal); planarJoint->updatePlane(); }
void dJointSetTransmissionAxis2( dJointID j, dReal x, dReal y, dReal z ) { dxJointTransmission* joint = static_cast<dxJointTransmission*>(j); dUASSERT( joint, "bad joint argument" ); dUASSERT(joint->mode = dTransmissionIntersectingAxes, "can't set individual axes in current mode" ); if (joint->node[1].body) { dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]); dNormalize3(joint->axes[1]); } joint->update = 1; }
void testPlaneSpace() { HEADER; dVector3 n,p,q; int bad = 0; for (int i=0; i<1000; i++) { dMakeRandomVector (n,3,1.0); dNormalize3 (n); dPlaneSpace (n,p,q); if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1; if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1; if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1; if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1; if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1; } printf ("\t%s\n", bad ? "FAILED" : "passed"); }
// check for separation between box edge and cylinder circle edge int _cldTestEdgeCircleAxis( sCylinderBoxData& cData, const dVector3 &vCenterPoint, const dVector3 &vVx0, const dVector3 &vVx1, int iAxis ) { // calculate direction of edge dVector3 vDirEdge; dVector3Subtract(vVx1,vVx0,vDirEdge); dNormalize3(vDirEdge); // starting point of edge dVector3 vEStart; dVector3Copy(vVx0,vEStart);; // calculate angle cosine between cylinder axis and edge dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis); // if edge is perpendicular to cylinder axis if(dFabs(fdot2) < 1e-5f) { // this can't be separating axis, because edge is parallel to circle plane return 1; } // find point of intersection between edge line and circle plane dVector3 vTemp1; dVector3Subtract(vCenterPoint,vEStart,vTemp1); dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis); dVector3 vpnt; vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2); vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2); vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2); // find tangent vector on circle with same center (vCenterPoint) that // touches point of intersection (vpnt) dVector3 vTangent; dVector3Subtract(vCenterPoint,vpnt,vTemp1); dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent); // find vector orthogonal both to tangent and edge direction dVector3 vAxis; dVector3Cross(vTangent,vDirEdge,vAxis); // use that vector as separating axis return _cldTestAxis( cData, vAxis, iAxis ); }
int test_plane_point_depth() { int j; dVector3 n,p,q,a,b; // n = plane normal dReal d; dSimpleSpace space(0); dGeomID plane = dCreatePlane (0,0,0,1,0); dSpaceAdd (space,plane); // ********** make a random plane for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; dNormalize3 (n); d = dRandReal() - 0.5; dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); dPlaneSpace (n,p,q); // ********** test point on plane has depth 0 a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = 0; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED(); // ********** test arbitrary depth point a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = dRandReal() - 0.5; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol) FAILED(); // ********** test depth-1 point a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = -1; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED(); PASSED(); }
// intersection test between edge and circle bool _cldTestCircleToEdgeAxis(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) { // calculate direction of edge dVector3 vkl; dVector3Subtract( vVx1 , vVx0 , vkl); dNormalize3(vkl); // starting point of edge dVector3 vol; dVector3Copy(vVx0,vol); // calculate angle cosine between cylinder axis and edge dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); // if edge is perpendicular to cylinder axis if(dFabs(fdot2)<REAL(1e-5)) { // this can't be separating axis, because edge is parallel to circle plane return true; } // find point of intersection between edge line and circle plane dVector3 vTemp; dVector3Subtract(vCenterPoint,vol,vTemp); dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1); dVector3 vpnt;// = vol + vkl * (fdot1/fdot2); vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2; vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2; vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2; // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt) dVector3 vTangent; dVector3Subtract(vCenterPoint,vpnt,vTemp); dVector3Cross(vTemp,vCylinderAxis1,vTangent); // find vector orthogonal both to tangent and edge direction dVector3 vAxis; dVector3Cross(vTangent,vkl,vAxis); // use that vector as separating axis return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis ); }
void dJointSetTransmissionAxis( dJointID j, dReal x, dReal y, dReal z ) { dxJointTransmission* joint = static_cast<dxJointTransmission*>(j); int i; dUASSERT( joint, "bad joint argument" ); dUASSERT(joint->mode == dTransmissionParallelAxes || joint->mode == dTransmissionChainDrive , "axes must be set individualy in current mode" ); for (i = 0 ; i < 2 ; i += 1) { if (joint->node[i].body) { dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]); dNormalize3(joint->axes[i]); } } joint->update = 1; }
// compute the 3 axes in global coordinates void dxJointAMotor::computeGlobalAxes( dVector3 ax[3] ) { if ( mode == dAMotorEuler ) { // special handling for euler mode dMULTIPLY0_331( ax[0], node[0].body->posr.R, axis[0] ); if ( node[1].body ) { dMULTIPLY0_331( ax[2], node[1].body->posr.R, axis[2] ); } else { ax[2][0] = axis[2][0]; ax[2][1] = axis[2][1]; ax[2][2] = axis[2][2]; } dCROSS( ax[1], = , ax[2], ax[0] ); dNormalize3( ax[1] ); }
void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az) { dVector3 n,p,q; n[0] = ax; n[1] = ay; n[2] = az; dNormalize3 (n); dPlaneSpace (n,p,q); _R(0,0) = p[0]; _R(1,0) = p[1]; _R(2,0) = p[2]; _R(0,1) = q[0]; _R(1,1) = q[1]; _R(2,1) = q[2]; _R(0,2) = n[0]; _R(1,2) = n[1]; _R(2,2) = n[2]; _R(0,3) = REAL(0.0); _R(1,3) = REAL(0.0); _R(2,3) = REAL(0.0); }
void testNormalize3() { HEADER; int i,j,bad=0; dVector3 n1,n2; for (i=0; i<1000; i++) { dMakeRandomVector (n1,3,1.0); for (j=0; j<3; j++) n2[j]=n1[j]; dNormalize3 (n2); if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1; if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2; if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4; if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8; if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16; if (bad) { printf ("\tFAILED (code=%x)\n",bad); return; } } printf ("\tpassed\n"); }
void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); g->recomputePosr(); dReal* rot = g->final_posr->R; dReal* pos = g->final_posr->pos; dVector3 n; pos[0] = px; pos[1] = py; pos[2] = pz; n[0] = dx; n[1] = dy; n[2] = dz; dNormalize3(n); rot[0*4+2] = n[0]; rot[1*4+2] = n[1]; rot[2*4+2] = n[2]; dGeomMoved (g); }
void dJointSetLMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z ) { dxJointLMotor* joint = ( dxJointLMotor* )j; //for now we are ignoring rel! dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 ); checktype( joint, LMotor ); if ( anum < 0 ) anum = 0; if ( anum > 2 ) anum = 2; if ( !joint->node[1].body && rel == 2 ) rel = 1; //ref 1 joint->rel[anum] = rel; dVector3 r; r[0] = x; r[1] = y; r[2] = z; r[3] = 0; if ( rel > 0 ) { if ( rel == 1 ) { dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r ); } else { //second body has to exists thanks to ref 1 line dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r ); } } else { joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; joint->axis[anum][2] = r[2]; } dNormalize3( joint->axis[anum] ); }
static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, dContactGeom *contact, int mode) { dVector3 q; q[0] = ray->final_posr->pos[0] - sphere_pos[0]; q[1] = ray->final_posr->pos[1] - sphere_pos[1]; q[2] = ray->final_posr->pos[2] - sphere_pos[2]; dReal B = dCalcVectorDot3_14(q,ray->final_posr->R+2); dReal C = dCalcVectorDot3(q,q) - radius*radius; // note: if C <= 0 then the start of the ray is inside the sphere dReal k = B*B - C; if (k < 0) return 0; k = dSqrt(k); dReal alpha; if (mode && C >= 0) { alpha = -B + k; if (alpha < 0) return 0; } else { alpha = -B - k; if (alpha < 0) { alpha = -B + k; if (alpha < 0) return 0; } } if (alpha > ray->length) return 0; contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0); contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]); contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]); contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]); dNormalize3 (contact->normal); contact->depth = alpha; return 1; }
void TestOneTriangleVsCylinder( sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, const bool bDoubleSided) { // calculate triangle normal dVector3Subtract( v2 , v1 ,cData.vE1); dVector3 vTemp; dVector3Subtract( v0 , v1 ,vTemp); dVector3Cross(cData.vE1 , vTemp , cData.vNormal ); dNormalize3( cData.vNormal); // create plane from triangle //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); dReal plDistance = -dVector3Dot(v0, cData.vNormal); dVector4 plTrianglePlane; dConstructPlane( cData.vNormal,plDistance,plTrianglePlane); // calculate sphere distance to plane dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane); // Sphere must be over positive side of triangle if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) { // if not don't generate contacts return; } dVector3 vPnt0; dVector3 vPnt1; dVector3 vPnt2; if (fDistanceCylinderCenterToPlane < REAL(0.0) ) { // flip it dVector3Copy(v0 , vPnt0); dVector3Copy(v1 , vPnt2); dVector3Copy(v2 , vPnt1); } else { dVector3Copy(v0 , vPnt0); dVector3Copy(v1 , vPnt1); dVector3Copy(v2 , vPnt2); } cData.fBestDepth = MAX_REAL; // do intersection test and find best separating axis if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) ) { // if not found do nothing return; } // if best separation axis is not found if ( cData.iBestAxis == 0 ) { // this should not happen (we should already exit in that case) dIASSERT(false); // do nothing return; } dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis ); // choose which clipping method are we going to apply if (dFabs(fdot) < REAL(0.9) ) { if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2)) { return; } } else { _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2); } }
// simulation loop static void simLoop (int pause) { static bool todo = false; if ( todo ) { // DEBUG static int cnt = 0; ++cnt; if (cnt == 5) command ( 'q' ); if (cnt == 10) dsStop(); } if (!pause) { double simstep = 0.01; // 10ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf (dt/simstep); if (!nrofsteps) nrofsteps = 1; for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldStep (world, simstep); dJointGroupEmpty (contactgroup); } update(); dReal radius, length; dsSetTexture (DS_WOOD); drawBox (geom[W], 1,1,0); drawBox (geom[EXT], 0,1,0); dVector3 anchorPos; dReal ang1 = 0; dReal ang2 = 0; dVector3 axisP, axisR1, axisR2; if ( dJointTypePU == type ) { dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); ang1 = pu->getAngle1(); ang2 = pu->getAngle2(); pu->getAxis1 (axisR1); pu->getAxis2 (axisR2); pu->getAxisP (axisP); dJointGetPUAnchor (pu->id(), anchorPos); } else if ( dJointTypePR == type ) { dPRJoint *pr = dynamic_cast<dPRJoint *> (joint); pr->getAxis1 (axisP); pr->getAxis2 (axisR1); dJointGetPRAnchor (pr->id(), anchorPos); } // Draw the axisR if ( geom[INT] ) { dsSetColor (1,0,1); dVector3 l; dGeomBoxGetLengths (geom[INT], l); const dReal *rotBox = dGeomGetRotation (geom[W]); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i]; dsDrawBox (pos, rotBox, l); } dsSetTexture (DS_CHECKERED); if ( geom[AXIS1] ) { dQuaternion q, qAng; dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dGeomGetQuaternion (geom[AXIS1], q); dQuaternion qq; dQMultiply1 (qq, qAng, q); dMatrix3 R; dQtoR (qq,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length); dsSetColor (1,0,0); dsDrawCylinder (anchorPos, R, length, radius); } if ( dJointTypePU == type && geom[AXIS2] ) { //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); dQuaternion q, qAng, qq, qq1; dGeomGetQuaternion (geom[AXIS2], q); dQFromAxisAndAngle (qAng, 0, 1, 0, ang2); dQMultiply1 (qq, qAng, q); dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dQMultiply1 (qq1, qAng, qq); dMatrix3 R; dQtoR (qq1,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length); dsSetColor (0,0,1); dsDrawCylinder (anchorPos, R, length, radius); } dsSetTexture (DS_WOOD); // Draw the anchor if ( geom[ANCHOR] ) { dsSetColor (1,1,1); dVector3 l; dGeomBoxGetLengths (geom[ANCHOR], l); const dReal *rotBox = dGeomGetRotation (geom[D]); const dReal *posBox = dGeomGetPosition (geom[D]); dVector3 e; for (int i=0; i<3; ++i) e[i] = posBox[i] - anchorPos[i]; dNormalize3 (e); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i]; dsDrawBox (pos, rotBox, l); } drawBox (geom[D], 1,1,0); } }
// clip and generate contacts static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { // if we have edge/edge intersection if ( iBestAxis > 4 ) { dVector3 vub,vPb,vPa; SET(vPa,vHullBoxPos); // calculate point on box edge for( int i=0; i<3; i++) { dVector3 vRotCol; GETCOL(mHullBoxRot,i,vRotCol); dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f; vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0]; vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1]; vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2]; } int iEdge = (iBestAxis-5)%3; // decide which edge is on triangle if ( iEdge == 0 ) { SET(vPb,v0); SET(vub,vE0); } else if ( iEdge == 1) { SET(vPb,v2); SET(vub,vE1); } else { SET(vPb,v1); SET(vub,vE2); } // setup direction parameter for face edge dNormalize3(vub); dReal fParam1, fParam2; // setup direction parameter for box edge dVector3 vua; int col=(iBestAxis-5)/3; GETCOL(mHullBoxRot,col,vua); // find two closest points on both edges _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 ); vPa[0] += vua[0]*fParam1; vPa[1] += vua[1]*fParam1; vPa[2] += vua[2]*fParam1; vPb[0] += vub[0]*fParam2; vPb[1] += vub[1]*fParam2; vPb[2] += vub[2]*fParam2; // calculate collision point dVector3 vPntTmp; ADD(vPa,vPb,vPntTmp); vPntTmp[0]*=0.5f; vPntTmp[1]*=0.5f; vPntTmp[2]*=0.5f; // generate contact point between two closest points #ifdef ORIG if (ctContacts < (iFlags & 0x0ffff)) { dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); Contact->depth = fBestDepth; SET(Contact->normal,vBestNormal); SET(Contact->pos,vPntTmp); Contact->g1 = Geom1; Contact->g2 = Geom2; ctContacts++; } #endif GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, vPntTmp, vBestNormal, fBestDepth, ctContacts); // if triangle is the referent face then clip box to triangle face } else if ( iBestAxis == 1 ) { dVector3 vNormal2; vNormal2[0]=-vBestNormal[0]; vNormal2[1]=-vBestNormal[1]; vNormal2[2]=-vBestNormal[2]; // vNr is normal in box frame, pointing from triangle to box dMatrix3 mTransposed; mTransposed[0*4+0]=mHullBoxRot[0*4+0]; mTransposed[0*4+1]=mHullBoxRot[1*4+0]; mTransposed[0*4+2]=mHullBoxRot[2*4+0]; mTransposed[1*4+0]=mHullBoxRot[0*4+1]; mTransposed[1*4+1]=mHullBoxRot[1*4+1]; mTransposed[1*4+2]=mHullBoxRot[2*4+1]; mTransposed[2*4+0]=mHullBoxRot[0*4+2]; mTransposed[2*4+1]=mHullBoxRot[1*4+2]; mTransposed[2*4+2]=mHullBoxRot[2*4+2]; dVector3 vNr; vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2]; vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2]; vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2]; dVector3 vAbsNormal; vAbsNormal[0] = dFabs( vNr[0] ); vAbsNormal[1] = dFabs( vNr[1] ); vAbsNormal[2] = dFabs( vNr[2] ); // get closest face from box int iB0, iB1, iB2; if (vAbsNormal[1] > vAbsNormal[0]) { if (vAbsNormal[1] > vAbsNormal[2]) { iB1 = 0; iB0 = 1; iB2 = 2; } else { iB1 = 0; iB2 = 1; iB0 = 2; } } else { if (vAbsNormal[0] > vAbsNormal[2]) { iB0 = 0; iB1 = 1; iB2 = 2; } else { iB1 = 0; iB2 = 1; iB0 = 2; } } // Here find center of box face we are going to project dVector3 vCenter; dVector3 vRotCol; GETCOL(mHullBoxRot,iB0,vRotCol); if (vNr[iB0] > 0) { vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0]; vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1]; vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2]; } else { vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0]; vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1]; vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2]; } // Here find 4 corner points of box dVector3 avPoints[4]; dVector3 vRotCol2; GETCOL(mHullBoxRot,iB1,vRotCol); GETCOL(mHullBoxRot,iB2,vRotCol2); for(int x=0;x<3;x++) { avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]); } // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes) dVector3 avTempArray1[9]; dVector3 avTempArray2[9]; dVector4 plPlane; int iTempCnt1=0; int iTempCnt2=0; // zeroify vectors - necessary? for(int i=0; i<9; i++) { avTempArray1[i][0]=0; avTempArray1[i][1]=0; avTempArray1[i][2]=0; avTempArray2[i][0]=0; avTempArray2[i][1]=0; avTempArray2[i][2]=0; } // Normal plane dVector3 vTemp; vTemp[0]=-vN[0]; vTemp[1]=-vN[1]; vTemp[2]=-vN[2]; dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane ); // Plane p0 dVector3 vTemp2; SUBTRACT(v1,v0,vTemp2); dCROSS(vTemp,=,vN,vTemp2); dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // Plane p1 SUBTRACT(v2,v1,vTemp2); dCROSS(vTemp,=,vN,vTemp2); dNormalize3(vTemp); SUBTRACT(v0,v2,vTemp2); CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp)); _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); // Plane p2 SUBTRACT(v0,v2,vTemp2); dCROSS(vTemp,=,vN,vTemp2); dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // END of clipping polygons // for each generated contact point for ( int i=0; i<iTempCnt2; i++ ) { // calculate depth dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]); // clamp depth to zero if (fTempDepth > 0) { fTempDepth = 0; } dVector3 vPntTmp; ADD(avTempArray2[i],v0,vPntTmp); #ifdef ORIG if (ctContacts < (iFlags & 0x0ffff)) { dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); Contact->depth = -fTempDepth; SET(Contact->normal,vBestNormal); SET(Contact->pos,vPntTmp); Contact->g1 = Geom1; Contact->g2 = Geom2; ctContacts++; } #endif GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, vPntTmp, vBestNormal, -fTempDepth, ctContacts); } //dAASSERT(ctContacts>0); // if box face is the referent face, then clip triangle on box face } else { // 2 <= if iBestAxis <= 4
int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dSphereClass); dIASSERT (o2->type == dBoxClass); dIASSERT ((flags & NUMC_MASK) >= 1); // this is easy. get the sphere center `p' relative to the box, and then clip // that to the boundary of the box (call that point `q'). if q is on the // boundary of the box and |p-q| is <= sphere radius, they touch. // if q is inside the box, the sphere is inside the box, so set a contact // normal to push the sphere to the closest box face. dVector3 l,t,p,q,r; dReal depth; int onborder = 0; dxSphere *sphere = (dxSphere*) o1; dxBox *box = (dxBox*) o2; contact->g1 = o1; contact->g2 = o2; p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0]; p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1]; p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2]; l[0] = box->side[0]*REAL(0.5); t[0] = dDOT14(p,o2->final_posr->R); if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; } if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; } l[1] = box->side[1]*REAL(0.5); t[1] = dDOT14(p,o2->final_posr->R+1); if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; } if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; } t[2] = dDOT14(p,o2->final_posr->R+2); l[2] = box->side[2]*REAL(0.5); if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; } if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; } if (!onborder) { // sphere center inside box. find closest face to `t' dReal min_distance = l[0] - dFabs(t[0]); int mini = 0; for (int i=1; i<3; i++) { dReal face_distance = l[i] - dFabs(t[i]); if (face_distance < min_distance) { min_distance = face_distance; mini = i; } } // contact position = sphere center contact->pos[0] = o1->final_posr->pos[0]; contact->pos[1] = o1->final_posr->pos[1]; contact->pos[2] = o1->final_posr->pos[2]; // contact normal points to closest face dVector3 tmp; tmp[0] = 0; tmp[1] = 0; tmp[2] = 0; tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0); dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp); // contact depth = distance to wall along normal plus radius contact->depth = min_distance + sphere->radius; return 1; } t[3] = 0; //@@@ hmmm dMULTIPLY0_331 (q,o2->final_posr->R,t); r[0] = p[0] - q[0]; r[1] = p[1] - q[1]; r[2] = p[2] - q[2]; depth = sphere->radius - dSqrt(dDOT(r,r)); if (depth < 0) return 0; contact->pos[0] = q[0] + o2->final_posr->pos[0]; contact->pos[1] = q[1] + o2->final_posr->pos[1]; contact->pos[2] = q[2] + o2->final_posr->pos[2]; contact->normal[0] = r[0]; contact->normal[1] = r[1]; contact->normal[2] = r[2]; dNormalize3 (contact->normal); contact->depth = depth; return 1; }
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ dIASSERT (Stride >= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (SphereGeom->type == dSphereClass); dIASSERT ((Flags & NUMC_MASK) >= 1); dxTriMesh* TriMesh = (dxTriMesh*)g1; // Init const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(); SphereCollider& Collider = pccColliderCache->_SphereCollider; const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); dReal Radius = dGeomSphereGetRadius(SphereGeom); // Sphere Sphere Sphere; Sphere.mCenter.x = Position[0]; Sphere.mCenter.y = Position[1]; Sphere.mCenter.z = Position[2]; Sphere.mRadius = Radius; Matrix4x4 amatrix; // TC results if (TriMesh->doSphereTC) { dxTriMesh::SphereTC* sphereTC = 0; for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){ if (TriMesh->SphereTCCache[i].Geom == SphereGeom){ sphereTC = &TriMesh->SphereTCCache[i]; break; } } if (!sphereTC){ TriMesh->SphereTCCache.push(dxTriMesh::SphereTC()); sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1]; sphereTC->Geom = SphereGeom; } // Intersect Collider.SetTemporalCoherence(true); Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, &MakeMatrix(TLPosition, TLRotation, amatrix)); } else { Collider.SetTemporalCoherence(false); Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, &MakeMatrix(TLPosition, TLRotation, amatrix)); } if (! Collider.GetContactStatus()) { // no collision occurred return 0; } // get results int TriCount = Collider.GetNbTouchedPrimitives(); const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); if (TriCount != 0){ if (TriMesh->ArrayCallback != null){ TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount); } int OutTriCount = 0; for (int i = 0; i < TriCount; i++){ if (OutTriCount == (Flags & NUMC_MASK)){ break; } const int TriIndex = Triangles[i]; dVector3 dv[3]; if (!Callback(TriMesh, SphereGeom, TriIndex)) continue; FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); dVector3& v0 = dv[0]; dVector3& v1 = dv[1]; dVector3& v2 = dv[2]; dVector3 vu; vu[0] = v1[0] - v0[0]; vu[1] = v1[1] - v0[1]; vu[2] = v1[2] - v0[2]; vu[3] = REAL(0.0); dVector3 vv; vv[0] = v2[0] - v0[0]; vv[1] = v2[1] - v0[1]; vv[2] = v2[2] - v0[2]; vv[3] = REAL(0.0); // Get plane coefficients dVector4 Plane; dCROSS(Plane, =, vu, vv); // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (!dSafeNormalize3(Plane)) { continue; } /* If the center of the sphere is within the positive halfspace of the * triangle's plane, allow a contact to be generated. * If the center of the sphere made it into the positive halfspace of a * back-facing triangle, then the physics update and/or velocity needs * to be adjusted (penetration has occured anyway). */ dReal side = dDOT(Plane,Position) - dDOT(Plane, v0); if(side < REAL(0.0)) { continue; } dReal Depth; dReal u, v; if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){ continue; // Sphere doesn't hit triangle } if (Depth < REAL(0.0)){ continue; // Negative depth does not produce a contact } dVector3 ContactPos; dReal w = REAL(1.0) - u - v; ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v); ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v); ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v); // Depth returned from GetContactData is depth along // contact point - sphere center direction // we'll project it to contact normal dVector3 dir; dir[0] = Position[0]-ContactPos[0]; dir[1] = Position[1]-ContactPos[1]; dir[2] = Position[2]-ContactPos[2]; dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir)); // Since Depth already had a requirement to be non-negative, // negative direction projections should not be allowed as well, // as otherwise the multiplication will result in negative contact depth. if (dirProj < REAL(0.0)){ continue; // Zero contact depth could be ignored } dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); Contact->pos[0] = ContactPos[0]; Contact->pos[1] = ContactPos[1]; Contact->pos[2] = ContactPos[2]; Contact->pos[3] = REAL(0.0); // Using normal as plane (reversed) Contact->normal[0] = -Plane[0]; Contact->normal[1] = -Plane[1]; Contact->normal[2] = -Plane[2]; Contact->normal[3] = REAL(0.0); Contact->depth = Depth * dirProj; //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance #if !defined MERGECONTACTS // Merge all contacts into 1 Contact->g1 = TriMesh; Contact->g2 = SphereGeom; Contact->side2 = -1; #endif // Otherwise assigned later Contact->side1 = TriIndex; OutTriCount++; } #if defined MERGECONTACTS // Merge all contacts into 1 if (OutTriCount > 0){ dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride); Contact->g1 = TriMesh; Contact->g2 = SphereGeom; Contact->side2 = -1; if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){ dVector3 pos; pos[0] = Contact->pos[0]; pos[1] = Contact->pos[1]; pos[2] = Contact->pos[2]; dVector3 normal; normal[0] = Contact->normal[0] * Contact->depth; normal[1] = Contact->normal[1] * Contact->depth; normal[2] = Contact->normal[2] * Contact->depth; int TriIndex = Contact->side1; for (int i = 1; i < OutTriCount; i++){ dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride); pos[0] += TempContact->pos[0]; pos[1] += TempContact->pos[1]; pos[2] += TempContact->pos[2]; normal[0] += TempContact->normal[0] * TempContact->depth; normal[1] += TempContact->normal[1] * TempContact->depth; normal[2] += TempContact->normal[2] * TempContact->depth; TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1; } Contact->side1 = TriIndex; Contact->pos[0] = pos[0] / OutTriCount; Contact->pos[1] = pos[1] / OutTriCount; Contact->pos[2] = pos[2] / OutTriCount; // Remember to divide in square space. Contact->depth = dSqrt(dDOT(normal, normal) / OutTriCount); if (Contact->depth > dEpsilon) { // otherwise the normal is too small dVector3Copy(Contact->normal, normal); dNormalize3(Contact->normal); } // otherwise original Contact's normal would be used and it should be already normalized } return 1; } else return 0; #elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts if (OutTriCount != 0){ if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){ dVector3 Normal; dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride); Normal[0] = FirstContact->normal[0] * FirstContact->depth; Normal[1] = FirstContact->normal[1] * FirstContact->depth; Normal[2] = FirstContact->normal[2] * FirstContact->depth; Normal[3] = FirstContact->normal[3] * FirstContact->depth; for (int i = 1; i < OutTriCount; i++){ dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); Normal[0] += Contact->normal[0] * Contact->depth; Normal[1] += Contact->normal[1] * Contact->depth; Normal[2] += Contact->normal[2] * Contact->depth; Normal[3] += Contact->normal[3] * Contact->depth; } dNormalize3(Normal); for (int i = 0; i < OutTriCount; i++){ dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); Contact->normal[0] = Normal[0]; Contact->normal[1] = Normal[1]; Contact->normal[2] = Normal[2]; Contact->normal[3] = Normal[3]; } } return OutTriCount; } else return 0; #else // none of MERGECONTACTS and MERGECONTACTNORMALS // Just return return OutTriCount; #endif // MERGECONTACTS } else return 0;