void BodyCutForce(dBodyID body,float l_limit,float w_limit) { const dReal wa_limit=w_limit/fixed_step; const dReal* force= dBodyGetForce(body); dReal force_mag=dSqrt(dDOT(force,force)); //body mass dMass m; dBodyGetMass(body,&m); dReal force_limit =l_limit/fixed_step*m.mass; if(force_mag>force_limit) { dBodySetForce(body, force[0]/force_mag*force_limit, force[1]/force_mag*force_limit, force[2]/force_mag*force_limit ); } const dReal* torque=dBodyGetTorque(body); dReal torque_mag=dSqrt(dDOT(torque,torque)); if(torque_mag<0.001f) return; dMatrix3 tmp,invI,I; // compute inertia tensor in global frame dMULTIPLY2_333 (tmp,m.I,body->R); dMULTIPLY0_333 (I,body->R,tmp); // compute inverse inertia tensor in global frame dMULTIPLY2_333 (tmp,body->invI,body->R); dMULTIPLY0_333 (invI,body->R,tmp); //angular accel dVector3 wa; dMULTIPLY0_331(wa,invI,torque); dReal wa_mag=dSqrt(dDOT(wa,wa)); if(wa_mag>wa_limit) { //scale w for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag; dVector3 new_torqu; dMULTIPLY0_331(new_torqu,I,wa); dBodySetTorque ( body, new_torqu[0], new_torqu[1], new_torqu[2] ); } }
void CODEGeom::get_global_form_bt(Fmatrix& form) { dMULTIPLY0_331 ((dReal*)(&form.c),dGeomGetRotation(m_geom_transform),dGeomGetPosition(geom())); form.c.add(*((const Fvector*)dGeomGetPosition(m_geom_transform))); dMULTIPLY3_333 ((dReal*)(&form),dGeomGetRotation(m_geom_transform),dGeomGetRotation(geom())); //PHDynamicData::DMXtoFMX((dReal*)(&form),form); }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCCylinderClass) { dReal radius,length; dGeomCCylinderGetParams (g,&radius,&length); dsDrawCappedCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R); } }
void dxTriMesh::computeAABB() { const dxTriMeshData* d = Data; dVector3 c; const dMatrix3& R = final_posr->R; const dVector3& pos = final_posr->pos; dMULTIPLY0_331( c, R, d->AABBCenter ); dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) + dFabs(R[1] * Data->AABBExtents[1]) + dFabs(R[2] * Data->AABBExtents[2]); dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) + dFabs(R[5] * Data->AABBExtents[1]) + dFabs(R[6] * Data->AABBExtents[2]); dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) + dFabs(R[9] * Data->AABBExtents[1]) + dFabs(R[10] * Data->AABBExtents[2]); aabb[0] = c[0] + pos[0] - xrange; aabb[1] = c[0] + pos[0] + xrange; aabb[2] = c[1] + pos[1] - yrange; aabb[3] = c[1] + pos[1] + yrange; aabb[4] = c[2] + pos[2] - zrange; aabb[5] = c[2] + pos[2] + zrange; }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!draw_geom){ return; } if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } /* // cylinder option not yet implemented else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } */ else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void dxJointFixed::getInfo2 ( dxJoint::Info2 *info ) { int s = info->rowskip; // Three rows for orientation setFixedOrientation ( this, info, qrel, 3 ); // Three rows for position. // set jacobian info->J1l[0] = 1; info->J1l[s+1] = 1; info->J1l[2*s+2] = 1; info->erp = erp; info->cfm[0] = cfm; info->cfm[1] = cfm; info->cfm[2] = cfm; dVector3 ofs; dMULTIPLY0_331 ( ofs, node[0].body->posr.R, offset ); if ( node[1].body ) { dCROSSMAT ( info->J1a, ofs, s, + , - ); info->J2l[0] = -1; info->J2l[s+1] = -1; info->J2l[2*s+2] = -1; }
void dxTriMesh::computeAABB() { const dxTriMeshData* d = Data; dVector3 c; // @optimize: using SSE dMULTIPLY0_331( c, R, d->AABBCenter ); dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) + dFabs(R[1] * Data->AABBExtents[1]) + dFabs(R[2] * Data->AABBExtents[2]); dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) + dFabs(R[5] * Data->AABBExtents[1]) + dFabs(R[6] * Data->AABBExtents[2]); dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) + dFabs(R[9] * Data->AABBExtents[1]) + dFabs(R[10] * Data->AABBExtents[2]); aabb[0] = c[0] + pos[0] - xrange; aabb[1] = c[0] + pos[0] + xrange; aabb[2] = c[1] + pos[1] - yrange; aabb[3] = c[1] + pos[1] + yrange; aabb[4] = c[2] + pos[2] - zrange; aabb[5] = c[2] + pos[2] + zrange; }
void dxGeomTransform::computeFinalTx() { dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos); transform_posr.pos[0] += final_posr->pos[0]; transform_posr.pos[1] += final_posr->pos[1]; transform_posr.pos[2] += final_posr->pos[2]; dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R); }
void dxGeomTransform::computeFinalTx() { dMULTIPLY0_331 (final_pos,R,obj->pos); final_pos[0] += pos[0]; final_pos[1] += pos[1]; final_pos[2] += pos[2]; dMULTIPLY0_333 (final_R,R,obj->R); }
void CODEGeom::get_global_center_bt(Fvector& center) { center.set(*((const Fvector*)dGeomGetPosition(m_geom_transform))); dVector3 add; dMULTIPLY0_331 (add,dGeomGetRotation(m_geom_transform),dGeomGetPosition(geom())); center.x += add[0]; center.y += add[1]; center.z += add[2]; }
// 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] ); }
static void computeFinalTx(dGeomID geom_transform,dReal* final_pos,dReal* final_R) { R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform"); dGeomID obj=dGeomTransformGetGeom(geom_transform); const dReal *R =dGeomGetRotation(geom_transform); const dReal *pos=dGeomGetPosition(geom_transform); dMULTIPLY0_331 (final_pos,R,dGeomGetPosition(obj)); final_pos[0] += pos[0]; final_pos[1] += pos[1]; final_pos[2] += pos[2]; dMULTIPLY0_333 (final_R,R,dGeomGetRotation(obj)); }
static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, dxBody * const *body, dRealPtr invI) { int i,j; dRealMutablePtr iMJ_ptr = iMJ; dRealMutablePtr J_ptr = J; for (i=0; i<m; i++) { int b1 = jb[i*2]; int b2 = jb[i*2+1]; dReal k = body[b1]->invMass; for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); if (b2 >= 0) { k = body[b2]->invMass; for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); } J_ptr += 12; iMJ_ptr += 12; } }
void dxConvex::computeAABB() { // this can, and should be optimized dVector3 point; dMULTIPLY0_331 (point,final_posr->R,points); aabb[0] = point[0]+final_posr->pos[0]; aabb[1] = point[0]+final_posr->pos[0]; aabb[2] = point[1]+final_posr->pos[1]; aabb[3] = point[1]+final_posr->pos[1]; aabb[4] = point[2]+final_posr->pos[2]; aabb[5] = point[2]+final_posr->pos[2]; for(unsigned int i=3;i<(pointcount*3);i+=3) { dMULTIPLY0_331 (point,final_posr->R,&points[i]); aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]); aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]); aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]); aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]); aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]); aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]); } }
void setBall( dxJoint *joint, dxJoint::Info2 *info, dVector3 anchor1, dVector3 anchor2 ) { // anchor points in global coordinates with respect to body PORs. dVector3 a1, a2; int s = info->rowskip; // set jacobian info->J1l[0] = 1; info->J1l[s+1] = 1; info->J1l[2*s+2] = 1; dMULTIPLY0_331( a1, joint->node[0].body->posr.R, anchor1 ); dCROSSMAT( info->J1a, a1, s, -, + ); if ( joint->node[1].body ) { info->J2l[0] = -1; info->J2l[s+1] = -1; info->J2l[2*s+2] = -1; dMULTIPLY0_331( a2, joint->node[1].body->posr.R, anchor2 ); dCROSSMAT( info->J2a, a2, s, + , - ); }
void dRigidBodyArraySetRotation(dRigidBodyArrayID bodyArray, const dReal *Rs) { dBodyID center = bodyArray->center; const dReal *p0 = dBodyGetPosition(center); const dReal *R0 = dBodyGetRotation(center); dMatrix3 RsR0t; dMULTIPLY2_333(RsR0t, Rs, R0); for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) { dBodyID body = dRigidBodyArrayGet(bodyArray, i); const dReal *pi = dBodyGetPosition(body); const dReal *Ri = dBodyGetRotation(body); dMatrix3 R1; dMULTIPLY0_333(R1, RsR0t, Ri); dVector3 p1, pi_p0, R0RsR0t__pi_p0; dOP(pi_p0, -, pi, p0); dMULTIPLY0_331(R0RsR0t__pi_p0, RsR0t, pi_p0); dOP(p1, +, R0RsR0t__pi_p0, p0); dBodySetPosition(body, p1[0], p1[1], p1[2]); dBodySetRotation(body, R1); } }
EXPORT_C int dBoxBox (const dVector3 p1, const dMatrix3 R1, const dVector3 side1, const dVector3 p2, const dMatrix3 R2, const dVector3 side2, dVector3 normal, dReal *depth, int *return_code, int maxc, dContactGeom *contact, int skip) { const dReal fudge_factor = REAL(1.05); dVector3 p,pp,normalC; const dReal *normalR = 0; dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; int i,j,invert_normal,code; // get vector from centers of box 1 to box 2, relative to box 1 p[0] = p2[0] - p1[0]; p[1] = p2[1] - p1[1]; p[2] = p2[2] - p1[2]; dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 // get side lengths / 2 A[0] = dMUL(side1[0],REAL(0.5)); A[1] = dMUL(side1[1],REAL(0.5)); A[2] = dMUL(side1[2],REAL(0.5)); B[0] = dMUL(side2[0],REAL(0.5)); B[1] = dMUL(side2[1],REAL(0.5)); B[2] = dMUL(side2[2],REAL(0.5)); // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. // * find the depth of the penetration along the separating axis (s2) // * if this is the largest depth so far, record it. // the normal vector will be set to the separating axis with the smallest // depth. note: normalR is set to point to a column of R1 or R2 if that is // the smallest depth normal so far. otherwise normalR is 0 and normalC is // set to a vector relative to body 1. invert_normal is 1 if the sign of // the normal should be flipped. #define TST(expr1,expr2,norm,cc) \ s2 = dFabs(expr1) - (expr2); \ if (s2 > 0) return 0; \ if (s2 > s) { \ s = s2; \ normalR = norm; \ invert_normal = ((expr1) < 0); \ code = (cc); \ } s = -dInfinity; invert_normal = 0; code = 0; // separating axis = u1,u2,u3 TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1); TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2); TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3); // separating axis = v1,v2,v3 TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4); TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5); TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(A[2],Q33) + B[2]),R2+2,6); // note: cross product axes need to be scaled when s is computed. // normal (n1,n2,n3) is relative to box 1. #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ s2 = dFabs(expr1) - (expr2); \ if (s2 > 0) return 0; \ l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \ if (l > 0) { \ s2 = dDIV(s2,l); \ if (dMUL(s2,fudge_factor) > s) { \ s = s2; \ normalR = 0; \ normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \ invert_normal = ((expr1) < 0); \ code = (cc); \ } \ } // separating axis = u1 x (v1,v2,v3) TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7); TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8); TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9); // separating axis = u2 x (v1,v2,v3) TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10); TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11); TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12); // separating axis = u3 x (v1,v2,v3) TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13); TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14); TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15); #undef TST if (!code) return 0; // if we get to this point, the boxes interpenetrate. compute the normal // in global coordinates. if (normalR) { normal[0] = normalR[0]; normal[1] = normalR[4]; normal[2] = normalR[8]; } else { dMULTIPLY0_331 (normal,R1,normalC); } if (invert_normal) { normal[0] = -normal[0]; normal[1] = -normal[1]; normal[2] = -normal[2]; } *depth = -s; // compute contact point(s) if (code > 6) { // an edge from box 1 touches an edge from box 2. // find a point pa on the intersecting edge of box 1 dVector3 pa; dReal sign; for (i=0; i<3; i++) pa[i] = p1[i]; for (j=0; j<3; j++) { sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0); for (i=0; i<3; i++) pa[i] += dMUL(sign,dMUL(A[j],R1[i*4+j])); } // find a point pb on the intersecting edge of box 2 dVector3 pb; for (i=0; i<3; i++) pb[i] = p2[i]; for (j=0; j<3; j++) { sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0); for (i=0; i<3; i++) pb[i] += dMUL(sign,dMUL(B[j],R2[i*4+j])); } dReal alpha,beta; dVector3 ua,ub; for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha); for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta); for (i=0; i<3; i++) contact[0].pos[i] = dMUL(REAL(0.5),(pa[i]+pb[i])); contact[0].depth = *depth; *return_code = code; return 1; } // okay, we have a face-something intersection (because the separating // axis is perpendicular to a face). define face 'a' to be the reference // face (i.e. the normal vector is perpendicular to this) and face 'b' to be // the incident face (the closest face of the other box). const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb; if (code <= 3) { Ra = R1; Rb = R2; pa = p1; pb = p2; Sa = A; Sb = B; } else { Ra = R2; Rb = R1; pa = p2; pb = p1; Sa = B; Sb = A; } // nr = normal vector of reference face dotted with axes of incident box. // anr = absolute values of nr. dVector3 normal2,nr,anr; if (code <= 3) { normal2[0] = normal[0]; normal2[1] = normal[1]; normal2[2] = normal[2]; } else { normal2[0] = -normal[0]; normal2[1] = -normal[1]; normal2[2] = -normal[2]; } dMULTIPLY1_331 (nr,Rb,normal2); anr[0] = dFabs (nr[0]); anr[1] = dFabs (nr[1]); anr[2] = dFabs (nr[2]); // find the largest compontent of anr: this corresponds to the normal // for the indident face. the other axis numbers of the indicent face // are stored in a1,a2. int lanr,a1,a2; if (anr[1] > anr[0]) { if (anr[1] > anr[2]) { a1 = 0; lanr = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } else { if (anr[0] > anr[2]) { lanr = 0; a1 = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } // compute center point of incident face, in reference-face coordinates dVector3 center; if (nr[lanr] < 0) { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + dMUL(Sb[lanr],Rb[i*4+lanr]); } else { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - dMUL(Sb[lanr],Rb[i*4+lanr]); } // find the normal and non-normal axis numbers of the reference box int codeN,code1,code2; if (code <= 3) codeN = code-1; else codeN = code-4; if (codeN==0) { code1 = 1; code2 = 2; } else if (codeN==1) { code1 = 0; code2 = 2; } else { code1 = 0; code2 = 1; } // find the four corners of the incident face, in reference-face coordinates dReal quad[8]; // 2D coordinate of incident face (x,y pairs) dReal c1,c2,m11,m12,m21,m22; c1 = dDOT14 (center,Ra+code1); c2 = dDOT14 (center,Ra+code2); // optimize this? - we have already computed this data above, but it is not // stored in an easy-to-index format. for now it's quicker just to recompute // the four dot products. m11 = dDOT44 (Ra+code1,Rb+a1); m12 = dDOT44 (Ra+code1,Rb+a2); m21 = dDOT44 (Ra+code2,Rb+a1); m22 = dDOT44 (Ra+code2,Rb+a2); { dReal k1 = dMUL(m11,Sb[a1]); dReal k2 = dMUL(m21,Sb[a1]); dReal k3 = dMUL(m12,Sb[a2]); dReal k4 = dMUL(m22,Sb[a2]); quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; quad[2] = c1 - k1 + k3; quad[3] = c2 - k2 + k4; quad[4] = c1 + k1 + k3; quad[5] = c2 + k2 + k4; quad[6] = c1 + k1 - k3; quad[7] = c2 + k2 - k4; } // find the size of the reference face dReal rect[2]; rect[0] = Sa[code1]; rect[1] = Sa[code2]; // intersect the incident and reference faces dReal ret[16]; int n = intersectRectQuad (rect,quad,ret); if (n < 1) return 0; // this should never happen // convert the intersection points into reference-face coordinates, // and compute the contact position and depth for each point. only keep // those points that have a positive (penetrating) depth. delete points in // the 'ret' array as necessary so that 'point' and 'ret' correspond. dReal point[3*8]; // penetrating contact points dReal dep[8]; // depths for those points dReal det1 = dRecip(dMUL(m11,m22) - dMUL(m12,m21)); m11 = dMUL(m11,det1); m12 = dMUL(m12,det1); m21 = dMUL(m21,det1); m22 = dMUL(m22,det1); int cnum = 0; // number of penetrating contact points found for (j=0; j < n; j++) { dReal k1 = dMUL(m22,(ret[j*2]-c1)) - dMUL(m12,(ret[j*2+1]-c2)); dReal k2 = -dMUL(m21,(ret[j*2]-c1)) + dMUL(m11,(ret[j*2+1]-c2)); for (i=0; i<3; i++) point[cnum*3+i] = center[i] + dMUL(k1,Rb[i*4+a1]) + dMUL(k2,Rb[i*4+a2]); dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); if (dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; ret[cnum*2+1] = ret[j*2+1]; cnum++; } } if (cnum < 1) return 0; // this should never happen // we can't generate more contacts than we actually have if (maxc > cnum) maxc = cnum; if (maxc < 1) maxc = 1; if (cnum <= maxc) { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { dContactGeom *con = CONTACT(contact,skip*j); for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; con->depth = dep[j]; } } else { // we have more contacts than are wanted, some of them must be culled. // find the deepest point, it is always the first contact. int i1 = 0; dReal maxdepth = dep[0]; for (i=1; i<cnum; i++) { if (dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; } } int iret[8]; cullPoints (cnum,ret,maxc,i1,iret); for (j=0; j < maxc; j++) { dContactGeom *con = CONTACT(contact,skip*j); for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; con->depth = dep[iret[j]]; } cnum = maxc; } *return_code = code; return cnum; }
int dBoxBox (const dVector3 p1, const dMatrix3 R1, const dVector3 side1, const dVector3 p2, const dMatrix3 R2, const dVector3 side2, dVector3 normal, dReal *depth, int *return_code, int flags, dContactGeom *contact, int skip) { const dReal fudge_factor = REAL(1.05); dVector3 p,pp,normalC={0,0,0}; const dReal *normalR = 0; dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val; int i,j,invert_normal,code; // get vector from centers of box 1 to box 2, relative to box 1 p[0] = p2[0] - p1[0]; p[1] = p2[1] - p1[1]; p[2] = p2[2] - p1[2]; dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 // get side lengths / 2 A[0] = side1[0]*REAL(0.5); A[1] = side1[1]*REAL(0.5); A[2] = side1[2]*REAL(0.5); B[0] = side2[0]*REAL(0.5); B[1] = side2[1]*REAL(0.5); B[2] = side2[2]*REAL(0.5); // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. // * find the depth of the penetration along the separating axis (s2) // * if this is the largest depth so far, record it. // the normal vector will be set to the separating axis with the smallest // depth. note: normalR is set to point to a column of R1 or R2 if that is // the smallest depth normal so far. otherwise normalR is 0 and normalC is // set to a vector relative to body 1. invert_normal is 1 if the sign of // the normal should be flipped. do { #define TST(expr1,expr2,norm,cc) \ expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ s2 = dFabs(expr1_val) - (expr2); \ if (s2 > 0) return 0; \ if (s2 > s) { \ s = s2; \ normalR = norm; \ invert_normal = ((expr1_val) < 0); \ code = (cc); \ if (flags & CONTACTS_UNIMPORTANT) break; \ } s = -dInfinity; invert_normal = 0; code = 0; // separating axis = u1,u2,u3 TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); // separating axis = v1,v2,v3 TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); // note: cross product axes need to be scaled when s is computed. // normal (n1,n2,n3) is relative to box 1. #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ s2 = dFabs(expr1_val) - (expr2); \ if (s2 > 0) return 0; \ l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ if (l > 0) { \ s2 /= l; \ if (s2*fudge_factor > s) { \ s = s2; \ normalR = 0; \ normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ invert_normal = ((expr1_val) < 0); \ code = (cc); \ if (flags & CONTACTS_UNIMPORTANT) break; \ } \ } // We only need to check 3 edges per box // since parallel edges are equivalent. // separating axis = u1 x (v1,v2,v3) TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); // separating axis = u2 x (v1,v2,v3) TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); // separating axis = u3 x (v1,v2,v3) TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); #undef TST } while (0); if (!code) return 0; // if we get to this point, the boxes interpenetrate. compute the normal // in global coordinates. if (normalR) { normal[0] = normalR[0]; normal[1] = normalR[4]; normal[2] = normalR[8]; } else { dMULTIPLY0_331 (normal,R1,normalC); } if (invert_normal) { normal[0] = -normal[0]; normal[1] = -normal[1]; normal[2] = -normal[2]; } *depth = -s; // compute contact point(s) if (code > 6) { // An edge from box 1 touches an edge from box 2. // find a point pa on the intersecting edge of box 1 dVector3 pa; dReal sign; // Copy p1 into pa for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy? // Get world position of p2 into pa for (j=0; j<3; j++) { sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0); for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; } // find a point pb on the intersecting edge of box 2 dVector3 pb; // Copy p2 into pb for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy? // Get world position of p2 into pb for (j=0; j<3; j++) { sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0); for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; } dReal alpha,beta; dVector3 ua,ub; // Get direction of first edge for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; // Get direction of second edge for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; // Get closest points between edges (one at each) dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); for (i=0; i<3; i++) pa[i] += ua[i]*alpha; for (i=0; i<3; i++) pb[i] += ub[i]*beta; // Set the contact point as halfway between the 2 closest points for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); contact[0].depth = *depth; *return_code = code; return 1; } // okay, we have a face-something intersection (because the separating // axis is perpendicular to a face). define face 'a' to be the reference // face (i.e. the normal vector is perpendicular to this) and face 'b' to be // the incident face (the closest face of the other box). // Note: Unmodified parameter values are being used here const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb; if (code <= 3) { // One of the faces of box 1 is the reference face Ra = R1; // Rotation of 'a' Rb = R2; // Rotation of 'b' pa = p1; // Center (location) of 'a' pb = p2; // Center (location) of 'b' Sa = A; // Side Lenght of 'a' Sb = B; // Side Lenght of 'b' } else { // One of the faces of box 2 is the reference face Ra = R2; // Rotation of 'a' Rb = R1; // Rotation of 'b' pa = p2; // Center (location) of 'a' pb = p1; // Center (location) of 'b' Sa = B; // Side Lenght of 'a' Sb = A; // Side Lenght of 'b' } // nr = normal vector of reference face dotted with axes of incident box. // anr = absolute values of nr. /* The normal is flipped if necessary so it always points outward from box 'a', box 'b' is thus always the incident box */ dVector3 normal2,nr,anr; if (code <= 3) { normal2[0] = normal[0]; normal2[1] = normal[1]; normal2[2] = normal[2]; } else { normal2[0] = -normal[0]; normal2[1] = -normal[1]; normal2[2] = -normal[2]; } // Rotate normal2 in incident box opposite direction dMULTIPLY1_331 (nr,Rb,normal2); anr[0] = dFabs (nr[0]); anr[1] = dFabs (nr[1]); anr[2] = dFabs (nr[2]); // find the largest compontent of anr: this corresponds to the normal // for the incident face. the other axis numbers of the incident face // are stored in a1,a2. int lanr,a1,a2; if (anr[1] > anr[0]) { if (anr[1] > anr[2]) { a1 = 0; lanr = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } else { if (anr[0] > anr[2]) { lanr = 0; a1 = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } // compute center point of incident face, in reference-face coordinates dVector3 center; if (nr[lanr] < 0) { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; } else { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; } // find the normal and non-normal axis numbers of the reference box int codeN,code1,code2; if (code <= 3) codeN = code-1; else codeN = code-4; if (codeN==0) { code1 = 1; code2 = 2; } else if (codeN==1) { code1 = 0; code2 = 2; } else { code1 = 0; code2 = 1; } // find the four corners of the incident face, in reference-face coordinates dReal quad[8]; // 2D coordinate of incident face (x,y pairs) dReal c1,c2,m11,m12,m21,m22; c1 = dDOT14 (center,Ra+code1); c2 = dDOT14 (center,Ra+code2); // optimize this? - we have already computed this data above, but it is not // stored in an easy-to-index format. for now it's quicker just to recompute // the four dot products. m11 = dDOT44 (Ra+code1,Rb+a1); m12 = dDOT44 (Ra+code1,Rb+a2); m21 = dDOT44 (Ra+code2,Rb+a1); m22 = dDOT44 (Ra+code2,Rb+a2); { dReal k1 = m11*Sb[a1]; dReal k2 = m21*Sb[a1]; dReal k3 = m12*Sb[a2]; dReal k4 = m22*Sb[a2]; quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; quad[2] = c1 - k1 + k3; quad[3] = c2 - k2 + k4; quad[4] = c1 + k1 + k3; quad[5] = c2 + k2 + k4; quad[6] = c1 + k1 - k3; quad[7] = c2 + k2 - k4; } // find the size of the reference face dReal rect[2]; rect[0] = Sa[code1]; rect[1] = Sa[code2]; // intersect the incident and reference faces dReal ret[16]; int n = intersectRectQuad (rect,quad,ret); if (n < 1) return 0; // this should never happen // convert the intersection points into reference-face coordinates, // and compute the contact position and depth for each point. only keep // those points that have a positive (penetrating) depth. delete points in // the 'ret' array as necessary so that 'point' and 'ret' correspond. dReal point[3*8]; // penetrating contact points dReal dep[8]; // depths for those points dReal det1 = dRecip(m11*m22 - m12*m21); m11 *= det1; m12 *= det1; m21 *= det1; m22 *= det1; int cnum = 0; // number of penetrating contact points found for (j=0; j < n; j++) { dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); for (i=0; i<3; i++) point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); if (dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; ret[cnum*2+1] = ret[j*2+1]; cnum++; if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } if (cnum < 1) { return 0; // this should not happen, yet does at times (demo_plane2d single precision). } // we can't generate more contacts than we actually have int maxc = flags & NUMC_MASK; if (maxc > cnum) maxc = cnum; if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function if (cnum <= maxc) { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { dContactGeom *con = CONTACT(contact,skip*j); for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; con->depth = dep[j]; } } else { dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed // we have more contacts than are wanted, some of them must be culled. // find the deepest point, it is always the first contact. int i1 = 0; dReal maxdepth = dep[0]; for (i=1; i<cnum; i++) { if (dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; } } int iret[8]; cullPoints (cnum,ret,maxc,i1,iret); for (j=0; j < maxc; j++) { dContactGeom *con = CONTACT(contact,skip*j); for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; con->depth = dep[iret[j]]; } cnum = maxc; } *return_code = code; return cnum; }
void dInternalStepFast (dxWorld * /*world*/, dxBody * body[2], dReal * /*GI*/[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) { int i, j, k; dReal stepsize1 = dRecip (stepsize); int m = info.m; // nothing to do if no constraints. if (m <= 0) return; int nub = 0; if (info.nub == info.m) nub = m; // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same // format as J so we just go through the constraints in J multiplying by // the appropriate scalars and matrices. dReal JinvM[2 * 6 * 8]; //dSetZero (JinvM, 2 * m * 8); dReal *Jsrc = Jinfo.J1l; dReal *Jdst = JinvM; if (body[0]) { for (j = m - 1; j >= 0; j--) { for (k = 0; k < 3; k++) Jdst[k] = dMUL(Jsrc[k],body[0]->invMass); dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); Jsrc += 8; Jdst += 8; } } if (body[1]) { Jsrc = Jinfo.J2l; Jdst = JinvM + 8 * m; for (j = m - 1; j >= 0; j--) { for (k = 0; k < 3; k++) Jdst[k] = dMUL(Jsrc[k],body[1]->invMass); dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); Jsrc += 8; Jdst += 8; } } // now compute A = JinvM * J'. int mskip = dPAD (m); dReal A[6 * 8]; //dSetZero (A, 6 * 8); if (body[0]) { Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); if (body[1]) MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip); } else { if (body[1]) Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip); } // add cfm to the diagonal of A for (i = 0; i < m; i++) A[i * mskip + i] += dMUL(Jinfo.cfm[i],stepsize1); // compute the right hand side `rhs' dReal tmp1[16]; //dSetZero (tmp1, 16); // put v/h + invM*fe into tmp1 for (i = 0; i < 2; i++) { if (!body[i]) continue; for (j = 0; j < 3; j++) tmp1[i * 8 + j] = dMUL(body[i]->facc[j],body[i]->invMass) + dMUL(body[i]->lvel[j],stepsize1); dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); for (j = 0; j < 3; j++) tmp1[i * 8 + 4 + j] += dMUL(body[i]->avel[j],stepsize1); } // put J*tmp1 into rhs dReal rhs[6]; //dSetZero (rhs, 6); if (body[0]) { Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); if (body[1]) MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); } else { if (body[1]) Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); } // complete rhs for (i = 0; i < m; i++) rhs[i] = dMUL(Jinfo.c[i],stepsize1) - rhs[i]; #ifdef SLOW_LCP // solve the LCP problem and get lambda. // this will destroy A but that's okay dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); dReal lo[6], hi[6]; memcpy (lo, Jinfo.lo, m * sizeof (dReal)); memcpy (hi, Jinfo.hi, m * sizeof (dReal)); dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); #endif // compute the constraint force `cforce' // compute cforce = J'*lambda dJointFeedback *fb = joint->feedback; dReal cforce[16]; //dSetZero (cforce, 16); if (fb) { // the user has requested feedback on the amount of force that this // joint is applying to the bodies. we use a slightly slower // computation that splits out the force components and puts them // in the feedback structure. dReal data1[8], data2[8]; if (body[0]) { Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); dReal *cf1 = cforce; cf1[0] = (fb->f1[0] = data1[0]); cf1[1] = (fb->f1[1] = data1[1]); cf1[2] = (fb->f1[2] = data1[2]); cf1[4] = (fb->t1[0] = data1[4]); cf1[5] = (fb->t1[1] = data1[5]); cf1[6] = (fb->t1[2] = data1[6]); } if (body[1]) { Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); dReal *cf2 = cforce + 8; cf2[0] = (fb->f2[0] = data2[0]); cf2[1] = (fb->f2[1] = data2[1]); cf2[2] = (fb->f2[2] = data2[2]); cf2[4] = (fb->t2[0] = data2[4]); cf2[5] = (fb->t2[1] = data2[5]); cf2[6] = (fb->t2[2] = data2[6]); } } else { // no feedback is required, let's compute cforce the faster way if (body[0]) Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); if (body[1]) Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); } for (i = 0; i < 2; i++) { if (!body[i]) continue; for (j = 0; j < 3; j++) { body[i]->facc[j] += cforce[i * 8 + j]; body[i]->tacc[j] += cforce[i * 8 + 4 + j]; } } }
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; }
//#define DBG_BREAK bool CPHFracture::Update(CPHElement* element) { ////itterate through impacts & calculate dBodyID body=element->get_body(); //const Fvector& v_bodyvel=*((Fvector*)dBodyGetLinearVel(body)); CPHFracturesHolder* holder=element->FracturesHolder(); PH_IMPACT_STORAGE& impacts=holder->Impacts(); Fvector second_part_force,first_part_force,second_part_torque,first_part_torque; second_part_force.set(0.f,0.f,0.f); first_part_force.set(0.f,0.f,0.f); second_part_torque.set(0.f,0.f,0.f); first_part_torque.set(0.f,0.f,0.f); //const Fvector& body_local_pos=element->local_mass_Center(); const Fvector& body_global_pos=*(const Fvector*)dBodyGetPosition(body); Fvector body_to_first, body_to_second; body_to_first.set(*((const Fvector*)m_firstM.c));//,body_local_pos body_to_second.set(*((const Fvector*)m_secondM.c));//,body_local_pos //float body_to_first_smag=body_to_first.square_magnitude(); //float body_to_second_smag=body_to_second.square_magnitude(); int num=dBodyGetNumJoints(body); for(int i=0;i<num;i++) { bool applied_to_second=false; dJointID joint=dBodyGetJoint(body,i); dJointFeedback* feedback=dJointGetFeedback(joint); VERIFY2(feedback,"Feedback was not set!!!"); dxJoint* b_joint=(dxJoint*) joint; bool b_body_second=(b_joint->node[1].body==body); Fvector joint_position; if(dJointGetType(joint)==dJointTypeContact) { dxJointContact* c_joint=(dxJointContact*)joint; dGeomID first_geom=c_joint->contact.geom.g1; dGeomID second_geom=c_joint->contact.geom.g2; joint_position.set(*(Fvector*)c_joint->contact.geom.pos); if(dGeomGetClass(first_geom)==dGeomTransformClass) { first_geom=dGeomTransformGetGeom(first_geom); } if(dGeomGetClass(second_geom)==dGeomTransformClass) { second_geom=dGeomTransformGetGeom(second_geom); } dxGeomUserData* UserData; UserData=dGeomGetUserData(first_geom); if(UserData) { u16 el_position=UserData->element_position; //define if the contact applied to second part; if(el_position<element->numberOfGeoms()&& el_position>=m_start_geom_num&& el_position<m_end_geom_num&& first_geom==element->Geom(el_position)->geometry() ) applied_to_second=true; } UserData=dGeomGetUserData(second_geom); if(UserData) { u16 el_position=UserData->element_position; if(el_position<element->numberOfGeoms()&& el_position>=m_start_geom_num&& el_position<m_end_geom_num&& second_geom==element->Geom(el_position)->geometry() ) applied_to_second=true; } } else { CPHJoint* J = (CPHJoint*) dJointGetData(joint); if(!J)continue;//hack.. J->PSecondElement()->InterpolateGlobalPosition(&joint_position); CODEGeom* root_geom=J->RootGeom(); if(root_geom) { u16 el_position=root_geom->element_position(); if(element==J->PFirst_element()&& el_position<element->numberOfGeoms()&& el_position>=m_start_geom_num&& el_position<m_end_geom_num ) applied_to_second=true; } } //accomulate forces applied by joints to first and second parts Fvector body_to_joint; body_to_joint.sub(joint_position,body_global_pos); if(applied_to_second) { Fvector shoulder; shoulder.sub(body_to_joint,body_to_second); if(b_body_second) { Fvector joint_force; joint_force.set(*(const Fvector*)feedback->f2); second_part_force.add(joint_force); Fvector torque; torque.crossproduct(shoulder,joint_force); second_part_torque.add(torque); } else { Fvector joint_force; joint_force.set(*(const Fvector*)feedback->f1); second_part_force.add(joint_force); Fvector torque; torque.crossproduct(shoulder,joint_force); second_part_torque.add(torque); } } else { Fvector shoulder; shoulder.sub(body_to_joint,body_to_first); if(b_body_second) { Fvector joint_force; joint_force.set(*(const Fvector*)feedback->f2); first_part_force.add(joint_force); Fvector torque; torque.crossproduct(shoulder,joint_force); first_part_torque.add(torque); } else { Fvector joint_force; joint_force.set(*(const Fvector*)feedback->f1); first_part_force.add(joint_force); Fvector torque; torque.crossproduct(shoulder,joint_force); first_part_torque.add(torque); } } } PH_IMPACT_I i_i=impacts.begin(),i_e=impacts.end(); for(;i_i!=i_e;i_i++) { u16 geom = i_i->geom; if((geom>=m_start_geom_num&&geom<m_end_geom_num)) { Fvector force; force.set(i_i->force); force.mul(ph_console::phRigidBreakWeaponFactor); Fvector second_to_point; second_to_point.sub(body_to_second,i_i->point); //force.mul(30.f); second_part_force.add(force); Fvector torque; torque.crossproduct(second_to_point,force); second_part_torque.add(torque); } else { Fvector force; force.set(i_i->force); Fvector first_to_point; first_to_point.sub(body_to_first,i_i->point); //force.mul(4.f); first_part_force.add(force); Fvector torque; torque.crossproduct(first_to_point,force); second_part_torque.add(torque); } } Fvector gravity_force; gravity_force.set(0.f,-ph_world->Gravity()*m_firstM.mass,0.f); first_part_force.add(gravity_force); second_part_force.add(gravity_force); dMatrix3 glI1,glI2,glInvI,tmp; // compute inertia tensors in global frame dMULTIPLY2_333 (tmp,body->invI,body->R); dMULTIPLY0_333 (glInvI,body->R,tmp); dMULTIPLY2_333 (tmp,m_firstM.I,body->R); dMULTIPLY0_333 (glI1,body->R,tmp); dMULTIPLY2_333 (tmp,m_secondM.I,body->R); dMULTIPLY0_333 (glI2,body->R,tmp); //both parts have eqiual start angular vel same as have body so we ignore it //compute breaking torque ///break_torque=glI2*glInvI*first_part_torque-glI1*glInvI*second_part_torque+crossproduct(second_in_bone,second_part_force)-crossproduct(first_in_bone,first_part_force) Fvector break_torque,vtemp; dMULTIPLY0_331 ((float*)&break_torque,glInvI,(float*)&first_part_torque); dMULTIPLY0_331 ((float*)&break_torque,glI2,(float*)&break_torque); dMULTIPLY0_331 ((float*)&vtemp,glInvI,(float*)&second_part_torque); dMULTIPLY0_331 ((float*)&vtemp,glI1,(float*)&vtemp); break_torque.sub(vtemp); //Fvector first_in_bone,second_in_bone; //first_in_bone.sub(*((const Fvector*)m_firstM.c),m_pos_in_element); //second_in_bone.sub(*((const Fvector*)m_secondM.c),m_pos_in_element); //vtemp.crossproduct(second_in_bone,second_part_force); //break_torque.add(vtemp); //vtemp.crossproduct(first_in_bone,first_part_force); //break_torque.sub(vtemp); #ifdef DBG_BREAK float btm_dbg=break_torque.magnitude()*ph_console::phBreakCommonFactor/torque_factor; #endif if(break_torque.magnitude()*ph_console::phBreakCommonFactor>m_break_torque*torque_factor) { //m_break_torque.set(second_part_torque); m_pos_in_element.set(second_part_force); m_break_force=second_part_torque.x; m_break_torque=second_part_torque.y; m_add_torque_z=second_part_torque.z; m_breaked=true; #ifndef DBG_BREAK return m_breaked; #endif } Fvector break_force;//=1/(m1+m2)*(F1*m2-F2*m1)+r2xT2/(r2^2)-r1xT1/(r1^2) break_force.set(first_part_force); break_force.mul(m_secondM.mass); vtemp.set(second_part_force); vtemp.mul(m_firstM.mass); break_force.sub(vtemp); break_force.mul(1.f/element->getMass());//element->getMass()//body->mass.mass //vtemp.crossproduct(second_in_bone,second_part_torque); //break_force.add(vtemp); //vtemp.crossproduct(first_in_bone,first_part_torque); //break_force.sub(vtemp); float bfm=break_force.magnitude()*ph_console::phBreakCommonFactor; if(m_break_force<bfm) { second_part_force.mul(bfm/m_break_force); m_pos_in_element.set(second_part_force); //m_pos_in_element.add(break_force); m_break_force=second_part_torque.x; m_break_torque=second_part_torque.y; m_add_torque_z=second_part_torque.z; m_breaked=true; #ifndef DBG_BREAK return m_breaked; #endif } #ifdef DBG_BREAK Msg("bone_id %d break_torque - %f(max %f) break_force %f (max %f) breaked %d",m_bone_id,btm_dbg,m_break_torque,bfm,m_break_force,m_breaked); #endif return m_breaked; }
void _cldClipBoxToCylinder(sCylinderBoxData& cData ) { dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; // check which circle from cylinder we take for clipping if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) ) { // get top circle vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); vCylinderCircleNormal_Rel[0] = REAL(0.0); vCylinderCircleNormal_Rel[1] = REAL(0.0); vCylinderCircleNormal_Rel[2] = REAL(0.0); vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); } else { // get bottom circle vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); vCylinderCircleNormal_Rel[0] = REAL(0.0); vCylinderCircleNormal_Rel[1] = REAL(0.0); vCylinderCircleNormal_Rel[2] = REAL(0.0); vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); } // vNr is normal in Box frame, pointing from Cylinder to Box dVector3 vNr; dMatrix3 mBoxInv; // Find a way to use quaternion dMatrix3Inv(cData.mBoxRot,mBoxInv); dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr); dVector3 vAbsNormal; vAbsNormal[0] = dFabs( vNr[0] ); vAbsNormal[1] = dFabs( vNr[1] ); vAbsNormal[2] = dFabs( vNr[2] ); // find which face in box is closest to cylinder int iB0, iB1, iB2; // Different from Croteam's code if (vAbsNormal[1] > vAbsNormal[0]) { // 1 > 0 if (vAbsNormal[0]> vAbsNormal[2]) { // 0 > 2 -> 1 > 0 >2 iB0 = 1; iB1 = 0; iB2 = 2; } else { // 2 > 0-> Must compare 1 and 2 if (vAbsNormal[1] > vAbsNormal[2]) { // 1 > 2 -> 1 > 2 > 0 iB0 = 1; iB1 = 2; iB2 = 0; } else { // 2 > 1 -> 2 > 1 > 0; iB0 = 2; iB1 = 1; iB2 = 0; } } } else { // 0 > 1 if (vAbsNormal[1] > vAbsNormal[2]) { // 1 > 2 -> 0 > 1 > 2 iB0 = 0; iB1 = 1; iB2 = 2; } else { // 2 > 1 -> Must compare 0 and 2 if (vAbsNormal[0] > vAbsNormal[2]) { // 0 > 2 -> 0 > 2 > 1; iB0 = 0; iB1 = 2; iB2 = 1; } else { // 2 > 0 -> 2 > 0 > 1; iB0 = 2; iB1 = 0; iB2 = 1; } } } dVector3 vCenter; // find center of box polygon dVector3 vTemp; if (vNr[iB0] > 0) { dMat3GetCol(cData.mBoxRot,iB0,vTemp); vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2]; } else { dMat3GetCol(cData.mBoxRot,iB0,vTemp); vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2]; } // find the vertices of box polygon dVector3 avPoints[4]; dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; int i=0; for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++) { avTempArray1[i][0] = REAL(0.0); avTempArray1[i][1] = REAL(0.0); avTempArray1[i][2] = REAL(0.0); avTempArray2[i][0] = REAL(0.0); avTempArray2[i][1] = REAL(0.0); avTempArray2[i][2] = REAL(0.0); } dVector3 vAxis1, vAxis2; dMat3GetCol(cData.mBoxRot,iB1,vAxis1); dMat3GetCol(cData.mBoxRot,iB2,vAxis2); avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2]; avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2]; avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2]; avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2]; // transform box points to space of cylinder circle dMatrix3 mCylinderInv; dMatrix3Inv(cData.mCylinderRot,mCylinderInv); for(i=0; i<4; i++) { dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp); dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]); } int iTmpCounter1 = 0; int iTmpCounter2 = 0; dVector4 plPlane; // plane of cylinder that contains circle for intersection dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane); // Body of base circle of Cylinder int nCircleSegment = 0; for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++) { dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane); if (0 == (nCircleSegment % 2)) { dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); } else { dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); } dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS ); dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS ); } // back transform clipped points to absolute space dReal ftmpdot; dReal fTempDepth; dVector3 vPoint; if (nCircleSegment %2) { for( i=0; i<iTmpCounter2; i++) { dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, cData.vNormal); fTempDepth = cData.fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); Contact0->depth = fTempDepth; dVector3Copy(cData.vNormal,Contact0->normal); dVector3Copy(vPoint,Contact0->pos); Contact0->g1 = cData.gCylinder; Contact0->g2 = cData.gBox; dVector3Inv(Contact0->normal); cData.nContacts++; } } } else { for( i=0; i<iTmpCounter1; i++) { dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, cData.vNormal); fTempDepth = cData.fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); Contact0->depth = fTempDepth; dVector3Copy(cData.vNormal,Contact0->normal); dVector3Copy(vPoint,Contact0->pos); Contact0->g1 = cData.gCylinder; Contact0->g2 = cData.gBox; dVector3Inv(Contact0->normal); cData.nContacts++; } } } }
int dBoxBox2 (const btVector3 p1, const dMatrix3 R1, const btVector3 side1, const btVector3 p2, const dMatrix3 R2, const btVector3 side2, BoxBoxResults *results) { const btScalar fudge_factor = 1.05; btVector3 p,pp,normalC; normalC[0] = 0.f; normalC[1] = 0.f; normalC[2] = 0.f; const btScalar *normalR = 0; btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,normal[3],depth; int i,j,invert_normal,code; // get vector from centers of box 1 to box 2, relative to box 1 p[0] = p2[0] - p1[0]; p[1] = p2[1] - p1[1]; p[2] = p2[2] - p1[2]; dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 // get side lengths (already specified as half lengths) A[0] = side1[0]; A[1] = side1[1]; A[2] = side1[2]; B[0] = side2[0]; B[1] = side2[1]; B[2] = side2[2]; // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. // * find the depth of the penetration along the separating axis (s2) // * if this is the largest depth so far, record it. // the normal vector will be set to the separating axis with the smallest // depth. note: normalR is set to point to a column of R1 or R2 if that is // the smallest depth normal so far. otherwise normalR is 0 and normalC is // set to a vector relative to body 1. invert_normal is 1 if the sign of // the normal should be flipped. #define TST(expr1,expr2,norm,cc) \ s2 = btFabs(expr1) - (expr2); \ if (s2 > 0) return 0; \ if (s2 > s) { \ s = s2; \ normalR = norm; \ invert_normal = ((expr1) < 0); \ code = (cc); \ } s = -dInfinity; invert_normal = 0; code = 0; // separating axis = u1,u2,u3 TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); // separating axis = v1,v2,v3 TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); // note: cross product axes need to be scaled when s is computed. // normal (n1,n2,n3) is relative to box 1. #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ s2 = btFabs(expr1) - (expr2); \ if (s2 > SIMD_EPSILON) return 0; \ l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ if (l > SIMD_EPSILON) { \ s2 /= l; \ if (s2*fudge_factor > s) { \ s = s2; \ normalR = 0; \ normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ invert_normal = ((expr1) < 0); \ code = (cc); \ } \ } btScalar fudge2 = 1.0e-5f; Q11 += fudge2; Q12 += fudge2; Q13 += fudge2; Q21 += fudge2; Q22 += fudge2; Q23 += fudge2; Q31 += fudge2; Q32 += fudge2; Q33 += fudge2; // separating axis = u1 x (v1,v2,v3) TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); // separating axis = u2 x (v1,v2,v3) TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); // separating axis = u3 x (v1,v2,v3) TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); #undef TST if (!code) return 0; results->code = code; // if we get to this point, the boxes interpenetrate. compute the normal // in global coordinates. if (normalR) { normal[0] = normalR[0]; normal[1] = normalR[4]; normal[2] = normalR[8]; } else { dMULTIPLY0_331 (normal,R1,normalC); } if (invert_normal) { normal[0] = -normal[0]; normal[1] = -normal[1]; normal[2] = -normal[2]; } depth = -s; // compute contact point(s) if (code > 6) { // an edge from box 1 touches an edge from box 2. // find a point pa on the intersecting edge of box 1 btVector3 pa; btScalar sign; for (i=0; i<3; i++) pa[i] = p1[i]; for (j=0; j<3; j++) { sign = (dDOT14(normal,R1+j) > 0) ? 1.0 : -1.0; for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; } // find a point pb on the intersecting edge of box 2 btVector3 pb; for (i=0; i<3; i++) pb[i] = p2[i]; for (j=0; j<3; j++) { sign = (dDOT14(normal,R2+j) > 0) ? -1.0 : 1.0; for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; } btScalar alpha,beta; btVector3 ua,ub; for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); for (i=0; i<3; i++) pa[i] += ua[i]*alpha; for (i=0; i<3; i++) pb[i] += ub[i]*beta; { btVector3 pointInWorld; addContactPoint(results,normal,pb,depth); } return 1; } // okay, we have a face-something intersection (because the separating // axis is perpendicular to a face). define face 'a' to be the reference // face (i.e. the normal vector is perpendicular to this) and face 'b' to be // the incident face (the closest face of the other box). const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; if (code <= 3) { Ra = R1; Rb = R2; pa = p1; pb = p2; Sa = A; Sb = B; } else { Ra = R2; Rb = R1; pa = p2; pb = p1; Sa = B; Sb = A; } // nr = normal vector of reference face dotted with axes of incident box. // anr = absolute values of nr. btVector3 normal2,nr,anr; if (code <= 3) { normal2[0] = normal[0]; normal2[1] = normal[1]; normal2[2] = normal[2]; } else { normal2[0] = -normal[0]; normal2[1] = -normal[1]; normal2[2] = -normal[2]; } dMULTIPLY1_331 (nr,Rb,normal2); anr[0] = btFabs (nr[0]); anr[1] = btFabs (nr[1]); anr[2] = btFabs (nr[2]); // find the largest compontent of anr: this corresponds to the normal // for the indident face. the other axis numbers of the indicent face // are stored in a1,a2. int lanr,a1,a2; if (anr[1] > anr[0]) { if (anr[1] > anr[2]) { a1 = 0; lanr = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } else { if (anr[0] > anr[2]) { lanr = 0; a1 = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } // compute center point of incident face, in reference-face coordinates btVector3 center; if (nr[lanr] < 0) { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; } else { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; } // find the normal and non-normal axis numbers of the reference box int codeN,code1,code2; if (code <= 3) codeN = code-1; else codeN = code-4; if (codeN==0) { code1 = 1; code2 = 2; } else if (codeN==1) { code1 = 0; code2 = 2; } else { code1 = 0; code2 = 1; } // find the four corners of the incident face, in reference-face coordinates btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) btScalar c1,c2,m11,m12,m21,m22; c1 = dDOT14 (center,Ra+code1); c2 = dDOT14 (center,Ra+code2); // optimize this? - we have already computed this data above, but it is not // stored in an easy-to-index format. for now it's quicker just to recompute // the four dot products. m11 = dDOT44 (Ra+code1,Rb+a1); m12 = dDOT44 (Ra+code1,Rb+a2); m21 = dDOT44 (Ra+code2,Rb+a1); m22 = dDOT44 (Ra+code2,Rb+a2); { btScalar k1 = m11*Sb[a1]; btScalar k2 = m21*Sb[a1]; btScalar k3 = m12*Sb[a2]; btScalar k4 = m22*Sb[a2]; quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; quad[2] = c1 - k1 + k3; quad[3] = c2 - k2 + k4; quad[4] = c1 + k1 + k3; quad[5] = c2 + k2 + k4; quad[6] = c1 + k1 - k3; quad[7] = c2 + k2 - k4; } // find the size of the reference face btScalar rect[2]; rect[0] = Sa[code1]; rect[1] = Sa[code2]; // intersect the incident and reference faces btScalar ret[16]; int n = intersectRectQuad2 (rect,quad,ret); if (n < 1) return 0; // this should never happen // convert the intersection points into reference-face coordinates, // and compute the contact position and depth for each point. only keep // those points that have a positive (penetrating) depth. delete points in // the 'ret' array as necessary so that 'point' and 'ret' correspond. btScalar point[3*8]; // penetrating contact points btScalar dep[8]; // depths for those points btScalar det1 = 1.f/(m11*m22 - m12*m21); m11 *= det1; m12 *= det1; m21 *= det1; m22 *= det1; int cnum = 0; // number of penetrating contact points found for (j=0; j < n; j++) { btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); for (i=0; i<3; i++) point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); if (dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; ret[cnum*2+1] = ret[j*2+1]; cnum++; } } if (cnum < 1) return 0; // this should never happen // we can't generate more contacts than we actually have int maxc = 4; if (maxc > cnum) maxc = cnum; if (maxc < 1) maxc = 1; if (cnum <= maxc) { if (code<4) { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { btVector3 pointInWorld; for (i=0; i<3; i++) pointInWorld[i] = point[j*3+i] + pa[i]; addContactPoint(results,normal,pointInWorld,dep[j]); } } else { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { btVector3 pointInWorld; for (i=0; i<3; i++) pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; addContactPoint(results,normal,pointInWorld,dep[j]); } } } else { // we have more contacts than are wanted, some of them must be culled. // find the deepest point, it is always the first contact. int i1 = 0; btScalar maxdepth = dep[0]; for (i=1; i<cnum; i++) { if (dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; } } int iret[8]; cullPoints2 (cnum,ret,maxc,i1,iret); for (j=0; j < maxc; j++) { btVector3 posInWorld; for (i=0; i<3; i++) posInWorld[i] = point[iret[j]*3+i] + pa[i]; if (code<4) { addContactPoint(results, normal,posInWorld,dep[iret[j]]); } else { posInWorld[0] -= normal[0]*dep[iret[j]]; posInWorld[1] -= normal[1]*dep[iret[j]]; posInWorld[2] -= normal[2]*dep[iret[j]]; addContactPoint(results, normal,posInWorld,dep[iret[j]]); } } cnum = maxc; } return cnum; }
void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, const dVector3 c, const dMatrix3 R, const dVector3 side, dVector3 lret, dVector3 bret) { int i; // compute the start and delta of the line p1-p2 relative to the box. // we will do all subsequent computations in this box-relative coordinate // system. we have to do a translation and rotation for each point. dVector3 tmp,s,v; tmp[0] = p1[0] - c[0]; tmp[1] = p1[1] - c[1]; tmp[2] = p1[2] - c[2]; dMULTIPLY1_331 (s,R,tmp); tmp[0] = p2[0] - p1[0]; tmp[1] = p2[1] - p1[1]; tmp[2] = p2[2] - p1[2]; dMULTIPLY1_331 (v,R,tmp); // mirror the line so that v has all components >= 0 dVector3 sign; for (i=0; i<3; i++) { if (v[i] < 0) { s[i] = -s[i]; v[i] = -v[i]; sign[i] = -1; } else sign[i] = 1; } // compute v^2 dVector3 v2; v2[0] = v[0]*v[0]; v2[1] = v[1]*v[1]; v2[2] = v[2]*v[2]; // compute the half-sides of the box double h[3]; h[0] = side[0]; h[1] = side[1]; h[2] = side[2]; // region is -1,0,+1 depending on which side of the box planes each // coordinate is on. tanchor is the next t value at which there is a // transition, or the last one if there are no more. int region[3]; double tanchor[3]; // find the region and tanchor values for p1 for (i=0; i<3; i++) { if (v[i] > 0) { if (s[i] < -h[i]) { region[i] = -1; tanchor[i] = (-h[i]-s[i])/v[i]; } else { region[i] = (s[i] > h[i]); tanchor[i] = (h[i]-s[i])/v[i]; } } else { region[i] = 0; tanchor[i] = 2; // this will never be a valid tanchor } } // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point double t=0; double dd2dt = 0; for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i]; if (dd2dt >= 0) goto got_answer; do { // find the point on the line that is at the next clip plane boundary double next_t = 1; for (i=0; i<3; i++) { if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t) next_t = tanchor[i]; } // compute d|d|^2/dt for the next t double next_dd2dt = 0; for (i=0; i<3; i++) { next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]); } // if the sign of d|d|^2/dt has changed, solution = the crossover point if (next_dd2dt >= 0) { double m = (next_dd2dt-dd2dt)/(next_t - t); t -= dd2dt/m; goto got_answer; } // advance to the next anchor point / region for (i=0; i<3; i++) { if (tanchor[i] == next_t) { tanchor[i] = (h[i]-s[i])/v[i]; region[i]++; } } t = next_t; dd2dt = next_dd2dt; } while (t < 1); t = 1; got_answer: // compute closest point on the line for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1 // compute closest point on the box for (i=0; i<3; i++) { tmp[i] = sign[i] * (s[i] + t*v[i]); if (tmp[i] < -h[i]) tmp[i] = -h[i]; else if (tmp[i] > h[i]) tmp[i] = h[i]; } dMULTIPLY0_331 (s,R,tmp); for (i=0; i<3; i++) bret[i] = s[i] + c[i]; }
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) { dIASSERT( skip >= (int)sizeof( dContactGeom ) ); dIASSERT( o1->type == dTriMeshClass ); dIASSERT( o2->type == dPlaneClass ); dIASSERT ((flags & NUMC_MASK) >= 1); // Alias pointers to the plane and trimesh dxTriMesh* trimesh = (dxTriMesh*)( o1 ); dxPlane* plane = (dxPlane*)( o2 ); int contact_count = 0; // Cache the maximum contact count. const int contact_max = ( flags & NUMC_MASK ); // Cache trimesh position and rotation. const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh ); const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh ); // // For all triangles. // // Cache the triangle count. const int tri_count = trimesh->Data->Mesh.GetNbTriangles(); VertexPointers VP; ConversionArea VC; dReal alpha; dVector3 vertex; #if !defined(dSINGLE) || 1 dVector3 int_vertex; // Intermediate vertex for double precision mode. #endif // dSINGLE // For each triangle for ( int t = 0; t < tri_count; ++t ) { // Get triangle, which should also use callback. trimesh->Data->Mesh.GetTriangle( VP, t, VC); // For each vertex. for ( int v = 0; v < 3; ++v ) { // // Get Vertex // #if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation dMULTIPLY0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) ); #else // dDOUBLE || 1 // OPCODE data is in single precision format. int_vertex[ 0 ] = VP.Vertex[ v ]->x; int_vertex[ 1 ] = VP.Vertex[ v ]->y; int_vertex[ 2 ] = VP.Vertex[ v ]->z; dMULTIPLY0_331( vertex, trimesh_R, int_vertex ); #endif // dSINGLE/dDOUBLE vertex[ 0 ] += trimesh_pos[ 0 ]; vertex[ 1 ] += trimesh_pos[ 1 ]; vertex[ 2 ] += trimesh_pos[ 2 ]; // // Collision? // // If alpha < 0 then point is if front of plane. i.e. no contact // If alpha = 0 then the point is on the plane alpha = plane->p[ 3 ] - dDOT( plane->p, vertex ); // If alpha > 0 the point is behind the plane. CONTACT! if ( alpha > 0 ) { // Alias the contact dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip ); contact->pos[ 0 ] = vertex[ 0 ]; contact->pos[ 1 ] = vertex[ 1 ]; contact->pos[ 2 ] = vertex[ 2 ]; contact->normal[ 0 ] = plane->p[ 0 ]; contact->normal[ 1 ] = plane->p[ 1 ]; contact->normal[ 2 ] = plane->p[ 2 ]; contact->depth = alpha; contact->g1 = plane; contact->g2 = trimesh; ++contact_count; // All contact slots are full? if ( contact_count >= contact_max ) return contact_count; // <=== STOP HERE } } } // Return contact count. return contact_count; }
// copied from an OpenDE demo program void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (g == nullptr) return; if (dGeomIsSpace(g) != 0) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } else show_aabb = 0; if (show_aabb != 0) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } //<---- Convex Object else if (type == dConvexClass) { #if 0 dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); #else dsDrawConvex(pos,R, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_body) { dBodyID body = dGeomGetBody(g); if (body) { const dReal *bodypos = dBodyGetPosition (body); const dReal *bodyr = dBodyGetRotation (body); dReal bodySides[3] = { 0.1, 0.1, 0.1 }; dsSetColorAlpha(0,1,0,1); dsDrawBox(bodypos,bodyr,bodySides); } } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }