//return true if a collision will occur between [0..1] //false otherwise. If true, minTime contains the time of impact bool BU_VertexPoly::GetTimeOfImpact( const BU_Screwing& screwAB, const SimdPoint3& a, const SimdVector4& planeEq, SimdScalar &minTime,bool swapAB) { bool hit = false; // precondition: s=0 and w= 0 is catched by caller! if (TestFuzzyZero(screwAB.GetS()) && TestFuzzyZero(screwAB.GetW())) { return false; } //case w<>0 and s<> 0 const SimdScalar w=screwAB.GetW(); const SimdScalar s=screwAB.GetS(); SimdScalar coefs[4]; const SimdScalar p=planeEq[0]; const SimdScalar q=planeEq[1]; const SimdScalar r=planeEq[2]; const SimdScalar d=planeEq[3]; const SimdVector3 norm(p,q,r); BU_Polynomial polynomialSolver; int numroots = 0; //SimdScalar eps=1e-80f; //SimdScalar eps2=1e-100f; if (TestFuzzyZero(screwAB.GetS()) ) { //S = 0 , W <> 0 //ax^3+bx^2+cx+d=0 coefs[0]=0.; coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d); coefs[2]=-2*p*a.y()+2*q*a.x(); coefs[3]=p*a.x()+q*a.y()+r*a.z()-d; // numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]); numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]); } else { if (TestFuzzyZero(screwAB.GetW())) { // W = 0 , S <> 0 //pax+qay+r(az+st)=d SimdScalar dist = (d - a.dot(norm)); if (TestFuzzyZero(r)) { if (TestFuzzyZero(dist)) { // no hit } else { // todo a a' might hit sides of polygon T //printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n"); } } else { SimdScalar etoi = (dist)/(r*screwAB.GetS()); if (swapAB) etoi *= -1; if (etoi >= 0. && etoi <= minTime) { minTime = etoi; hit = true; } } } else { //ax^3+bx^2+cx+d=0 //degenerate coefficients mess things up :( SimdScalar ietsje = (r*s)/SimdTan(w/2.f); if (ietsje*ietsje < 0.01f) ietsje = 0.f; coefs[0]=ietsje;//(r*s)/tan(w/2.); coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d); coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.))); coefs[3]=p*a.x()+q*a.y()+r*a.z()-d; numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]); } } for (int i=0;i<numroots;i++) { SimdScalar tau = polynomialSolver.GetRoot(i); SimdScalar t = 2.f*SimdAtan(tau)/w; //tau = tan (wt/2) so 2*atan (tau)/w if (swapAB) { t *= -1.; } if (t>=0 && t<minTime) { minTime = t; hit = true; } } return hit; }
bool BU_CollisionPair::calcTimeOfImpact( const SimdTransform& fromA, const SimdTransform& toA, const SimdTransform& fromB, const SimdTransform& toB, CastResult& result) { SimdVector3 linvelA,angvelA; SimdVector3 linvelB,angvelB; SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA); SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB); SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin(); SimdQuaternion angularMotionA(0,0,0,1.f); SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin(); SimdQuaternion angularMotionB(0,0,0,1); result.m_fraction = 1.f; SimdTransform impactTransA; SimdTransform impactTransB; int index=0; SimdScalar toiUnscaled=result.m_fraction; const SimdScalar toiUnscaledLimit = result.m_fraction; SimdTransform a2w; a2w = fromA; SimdTransform b2w = fromB; /* debugging code { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = b2w * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ SimdTransform b2wp = b2w; b2wp.setOrigin(b2w.getOrigin() + linearMotionB); b2wp.setRotation( b2w.getRotation() + angularMotionB); impactTransB = b2wp; SimdTransform a2wp; a2wp.setOrigin(a2w.getOrigin()+ linearMotionA); a2wp.setRotation(a2w.getRotation()+angularMotionA); impactTransA = a2wp; SimdTransform a2winv; a2winv = a2w.inverse(); SimdTransform b2wpinv; b2wpinv = b2wp.inverse(); SimdTransform b2winv; b2winv = b2w.inverse(); SimdTransform a2wpinv; a2wpinv = a2wp.inverse(); //Redon's version with concatenated transforms SimdTransform relative; relative = b2w * b2wpinv * a2wp * a2winv; //relative = a2winv * a2wp * b2wpinv * b2w; SimdQuaternion qrel; relative.getBasis().getRotation(qrel); SimdVector3 linvel = relative.getOrigin(); if (linvel.length() < SCREWEPSILON) { linvel.setValue(0.,0.,0.); } SimdVector3 angvel; angvel[0] = 2.f * SimdAsin (qrel[0]); angvel[1] = 2.f * SimdAsin (qrel[1]); angvel[2] = 2.f * SimdAsin (qrel[2]); if (angvel.length() < SCREWEPSILON) { angvel.setValue(0.f,0.f,0.f); } //Redon's version with concatenated transforms m_screwing = BU_Screwing(linvel,angvel); SimdTransform w2s; m_screwing.LocalMatrix(w2s); SimdTransform s2w; s2w = w2s.inverse(); //impactTransA = a2w; //impactTransB = b2w; bool hit = false; if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW())) { //W = 0 , S = 0 , no collision //toi = 0; /* { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = impactTransB * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ return false;//don't continue moving within epsilon } #define EDGEEDGE #ifdef EDGEEDGE BU_EdgeEdge edgeEdge; //for all edged in A check agains all edges in B for (int ea = 0;ea < m_convexA->GetNumEdges();ea++) { SimdPoint3 pA0,pA1; m_convexA->GetEdge(ea,pA0,pA1); pA0= a2w * pA0;//in world space pA0 = w2s * pA0;//in screwing space pA1= a2w * pA1;//in world space pA1 = w2s * pA1;//in screwing space int numedgesB = m_convexB->GetNumEdges(); for (int eb = 0; eb < numedgesB;eb++) { { SimdPoint3 pB0,pB1; m_convexB->GetEdge(eb,pB0,pB1); pB0= b2w * pB0;//in world space pB0 = w2s * pB0;//in screwing space pB1= b2w * pB1;//in world space pB1 = w2s * pB1;//in screwing space SimdScalar lambda,mu; toiUnscaled = 1.; SimdVector3 edgeDirA(pA1-pA0); SimdVector3 edgeDirB(pB1-pB0); if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu)) { //printf("edgeedge potential hit\n"); if (toiUnscaled>=0) { if (toiUnscaled < toiUnscaledLimit) { //inside check is already done by checking the mu and gamma ! SimdPoint3 vtx = pA0+lambda * (pA1-pA0); SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); SimdPoint3 hitptWorld = s2w * hitpt; { if (toiUnscaled < result.m_fraction) result.m_fraction = toiUnscaled; hit = true; SimdVector3 hitNormal = edgeDirB.cross(edgeDirA); hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled); hitNormal.normalize(); //an approximated normal can be calculated by taking the cross product of both edges //take care of the sign ! SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld); if (dist > 0) hitNormalWorld *= -1; //todo: this is the wrong point, because b2winv is still at begin of motion // not at time-of-impact location! //bhitpt = b2winv * hitptWorld; // m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld); } } } } } index++; } }; #endif //EDGEEDGE #define VERTEXFACE #ifdef VERTEXFACE // for all vertices in A, for each face in B,do vertex-face { const int numvertsA = m_convexA->GetNumVertices(); for (int v=0;v<numvertsA;v++) //int v=3; { SimdPoint3 vtx; m_convexA->GetVertex(v,vtx); vtx = a2w * vtx;//in world space vtx = w2s * vtx;//in screwing space const int numplanesB = m_convexB->GetNumPlanes(); for (int p = 0 ; p < numplanesB; p++) //int p=2; { { SimdVector3 planeNorm; SimdPoint3 planeSupport; m_convexB->GetPlane(planeNorm,planeSupport,p); planeSupport = b2w * planeSupport;//transform to world space SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm; planeSupport = w2s * planeSupport ; //transform to screwing space planeNorm = w2s.getBasis() * planeNormWorld; planeNorm.normalize(); SimdScalar d = planeSupport.dot(planeNorm); SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); BU_VertexPoly vtxApolyB; toiUnscaled = 1.; if ((p==2) && (v==6)) { // printf("%f toiUnscaled\n",toiUnscaled); } if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false)) { if (toiUnscaled >= 0. ) { //not only collect the first point, get every contactpoint, later we have to check the //manifold properly! if (toiUnscaled <= toiUnscaledLimit) { // printf("toiUnscaled %f\n",toiUnscaled ); SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled); SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdPoint3 hitptWorld = s2w * hitpt; hitpt = b2winv * hitptWorld; //vertex has to be 'within' the facet's boundary if (m_convexB->IsInside(hitpt,m_tolerance)) { // m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld); if (toiUnscaled < result.m_fraction) result.m_fraction= toiUnscaled; hit = true; } } } } } index++; } } } // // for all vertices in B, for each face in A,do vertex-face //copy and pasted from all verts A -> all planes B so potential typos! //todo: make this into one method with a kind of 'swapped' logic // { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) //int v=0; { SimdPoint3 vtx; m_convexB->GetVertex(v,vtx); vtx = b2w * vtx;//in world space /* char buf[1000]; if (vtx.y() < 0.) { sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); */ vtx = w2s * vtx;//in screwing space const int numplanesA = m_convexA->GetNumPlanes(); for (int p = 0 ; p < numplanesA; p++) //int p=2; { { SimdVector3 planeNorm; SimdPoint3 planeSupport; m_convexA->GetPlane(planeNorm,planeSupport,p); planeSupport = a2w * planeSupport;//transform to world space SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm; planeSupport = w2s * planeSupport ; //transform to screwing space planeNorm = w2s.getBasis() * planeNormWorld; planeNorm.normalize(); SimdScalar d = planeSupport.dot(planeNorm); SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); BU_VertexPoly vtxBpolyA; toiUnscaled = 1.; if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true)) { if (toiUnscaled>=0.) { if (toiUnscaled < toiUnscaledLimit) { SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled); SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled); //SimdScalar len = hitNormal.length()-1; //assert( SimdFuzzyZero(len) ); SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdPoint3 hitptWorld = s2w * hitpt; hitpt = a2winv * hitptWorld; //vertex has to be 'within' the facet's boundary if (m_convexA->IsInside(hitpt,m_tolerance)) { // m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld); if (toiUnscaled <result.m_fraction) result.m_fraction = toiUnscaled; hit = true; } } } } } } index++; } } #endif// VERTEXFACE //the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame //in addition there are contact points from previous frames //we have to cleanup the manifold, using an additional epsilon/tolerance //as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point //else throw it away if (hit) { //try to avoid numerical drift on close contact if (result.m_fraction < 0.00001) { // printf("toiUnscaledMin< 0.00001\n"); impactTransA = a2w; impactTransB = b2w; } else { //SimdScalar vel = linearMotionB.length(); //todo: check this margin result.m_fraction *= 0.99f; //move B to new position impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB); SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction; ornB.normalize(); impactTransB.setRotation(ornB); //now transform A SimdTransform a2s,a2b; a2s.mult( w2s , a2w); a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction); a2s.multInverseLeft(w2s,a2s); a2b.multInverseLeft(b2w, a2s); //transform by motion B impactTransA.mult(impactTransB, a2b); //normalize rotation SimdQuaternion orn; impactTransA.getBasis().getRotation(orn); orn.normalize(); impactTransA.setBasis(SimdMatrix3x3(orn)); } } /* { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = impactTransB * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ return hit; }