GJK计算碰撞代码的应用//-----------------------------------------------------------------------------//Torque3D//Copyright(C)GarageGames.com,Inc.////Thecorealgorithmsinthisfilearebasedoncodewritten//byG.vandenBergenforhisinterferencedetectionlibrary,//"SOLID2.0"//-----------------------------------------------------------------------------#include"core/dataChunker.h"#include"collision/collision.h"#include"sceneGraph/sceneObject.h"#include"collision/convex.h"#include"collision/gjk.h"//----------------------------------------------------------------------------staticF32rel_error=1E-5f;//relativeerrorinthecomputeddistancestaticF32sTolerance=1E-3f;//DistancetolerancestaticF32sEpsilon2=1E-20f;//ZerolengthvectorstaticU32sIteration=15;//Stuckinaloop?S32num_iterations=0;S32num_irregularities=0;//----------------------------------------------------------------------------GjkCollisionState::GjkCollisionState(){a=b=0;}GjkCollisionState::~GjkCollisionState(){}//----------------------------------------------------------------------------voidGjkCollisionState::swap(){Convex*t=a;a=b;b=t;CollisionStateList*l=mLista;mLista=mListb;mListb=l;v.neg();}//----------------------------------------------------------------------------voidGjkCollisionState::compute_det(){//Dotnewpointwithcurrentsetfor(inti=0,bit=1;i0)returnfalse;}}returntrue;}//----------------------------------------------------------------------------inlineboolGjkCollisionState::closest(VectorF&v){compute_det();for(ints=bits;s;--s){if((s&bits)==s){if(valid(s|last_bit)){ bits=s|last_bit;if(bits!=15) compute_vector(bits,v); returntrue;}}}if(valid(last_bit)){bits=last_bit;v=y[last];returntrue;}returnfalse;}//----------------------------------------------------------------------------inlineboolGjkCollisionState::degenerate(constVectorF&w){for(inti=0,bit=1;imState=this;mListb=CollisionStateList::alloc();mListb-＞mState=this;}//----------------------------------------------------------------------------voidGjkCollisionState::reset(constMatrixF&a2w,constMatrixF&b2w){VectorFzero(0,0,0),sa,sb;a2w.mulP(a-＞support(zero),&sa);b2w.mulP(b-＞support(zero),&sb);v=sa-sb;dist=v.len();}//----------------------------------------------------------------------------voidGjkCollisionState::getCollisionInfo(constMatrixF&mat,Collision*info){AssertFatal(false,"GjkCollisionState::getCollisionInfo()-Thereremainscalingproblemshere.");//ThisassumesthattheshapesdonotintersectPoint3Fpa,pb;if(bits){getClosestPoints(pa,pb);mat.mulP(pa,&info-＞point);b-＞getTransform().mulP(pb,&pa);info-＞normal=info-＞point-pa;}else{mat.mulP(p[last],&info-＞point);info-＞normal=v;}info-＞normal.normalize();info-＞object=b-＞getObject();}voidGjkCollisionState::getClosestPoints(Point3F&p1,Point3F&p2){F32sum=0;p1.set(0,0,0);p2.set(0,0,0);for(inti=0,bit=1;isupport(va);a2w.mulP(p[last],&sa);VectorFvb,sb;w2b.mulV(v,&vb);q[last]=b-＞support(vb);b2w.mulP(q[last],&sb);VectorFw=sa-sb;if(mDot(v,w)＞0)returnfalse;if(degenerate(w)){++num_irregularities;returnfalse;}y[last]=w;all_bits=bits|last_bit;++num_iterations;if(!closest(v)||num_iterations＞sIteration){++num_irregularities;returnfalse;}}while(bitssEpsilon2);returntrue;}F32GjkCollisionState::distance(constMatrixF&a2w,constMatrixF&b2w,constF32dontCareDist,constMatrixF*_w2a,constMatrixF*_w2b){num_iterations=0;MatrixFw2a,w2b;if(_w2a==NULL||_w2b==NULL){w2a=a2w;w2b=b2w;w2a.inverse();w2b.inverse();}else{w2a=*_w2a;w2b=*_w2b;}reset(a2w,b2w);bits=0;all_bits=0;F32mu=0;do{nextBit();VectorFva,sa;w2a.mulV(-v,&va);p[last]=a-＞support(va);a2w.mulP(p[last],&sa);VectorFvb,sb;w2b.mulV(v,&vb);q[last]=b-＞support(vb);b2w.mulP(q[last],&sb);VectorFw=sa-sb;F32nm=mDot(v,w)/dist;if(nm＞mu)mu=nm;if(mu＞dontCareDist)returnmu;if(mFabs(dist-mu)sIteration){++num_irregularities;returndist;}y[last]=w;all_bits=bits|last_bit;if(!closest(v)){++num_irregularities;returndist;}dist=v.len();}while(bitssTolerance);if(bits==15&&mu＜=0)dist=0;returndist;}
2024/11/12 17:21:22
50KB
