SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
int i;
SimdVector3 supVec(0,0,0);
SimdScalar maxDot(-1e30f);
SimdVector3 vec = vec0;
SimdScalar lenSqr = vec.length2();
if (lenSqr < 0.0001f)
{
vec.setValue(1,0,0);
} else
{
float rlen = 1.f / SimdSqrt(lenSqr );
vec *= rlen;
}
SimdVector3 vtx;
SimdScalar newDot;
for (i=0;i<GetNumVertices();i++)
{
GetVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
void clientMotionFunc(int x,int y)
{
if (gPickingConstraintId && physicsEnvironmentPtr)
{
//move the constraint pivot
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//keep it at the same picking distance
SimdVector3 newRayTo = GetRayTo(x,y);
SimdVector3 eyePos(eye[0],eye[1],eye[2]);
SimdVector3 dir = newRayTo-eyePos;
dir.normalize();
dir *= gOldPickingDist;
SimdVector3 newPos = eyePos + dir;
p2p->SetPivotB(newPos);
}
}
}
void GLDebugDrawer::DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
{
if (m_debugMode > 0)
{
glBegin(GL_LINES);
glColor3f(color.getX(), color.getY(), color.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
}
SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec)const
{
SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
if ( GetMargin()!=0.f )
{
SimdVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
{
vecnorm.setValue(-1.f,-1.f,-1.f);
}
vecnorm.normalize();
supVertex+= GetMargin() * vecnorm;
}
return supVertex;
}
bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
{
const SimdVector3& p0 = verts[m_indices[0]];
SimdVector3 v1 = verts[m_indices[1]] - p0;
SimdVector3 v2 = verts[m_indices[2]] - p0;
SimdScalar v1dv1 = v1.length2();
SimdScalar v1dv2 = v1.dot(v2);
SimdScalar v2dv2 = v2.length2();
SimdScalar p0dv1 = p0.dot(v1);
SimdScalar p0dv2 = p0.dot(v2);
m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
//printf("m_det = %f\n",m_det);
//ASSERT(m_det >= 0.f);
if (m_det >= (SIMD_EPSILON*SIMD_EPSILON)) {
m_lambda1 = p0dv2 * v1dv2 - p0dv1 * v2dv2;
m_lambda2 = p0dv1 * v1dv2 - p0dv2 * v1dv1;
m_closest = p0 + (m_lambda1 * v1 + m_lambda2 * v2) / m_det;
m_dist2 = m_closest.length2();
return true;
}
return false;
}
void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
{
SimdTransform rayFromTrans,rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld);
//do culling based on aabb (rayFrom/rayTo)
SimdVector3 rayAabbMin = rayFromWorld;
SimdVector3 rayAabbMax = rayFromWorld;
rayAabbMin.setMin(rayToWorld);
rayAabbMax.setMax(rayToWorld);
/// brute force go over all objects. Once there is a broadphase, use that, or
/// add a raycast against aabb first.
std::vector<CollisionObject*>::iterator iter;
for (iter=m_collisionObjects.begin();
!(iter==m_collisionObjects.end()); iter++)
{
CollisionObject* collisionObject= (*iter);
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
//check aabb overlap
if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
{
RayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->m_collisionShape,
collisionObject->m_worldTransform,
resultCallback);
}
}
}
///
/// Debugging method CalcClosest calculates the closest point to the origin, using m_simplexSolver
///
void GL_Simplex1to4::CalcClosest(float* m)
{
SimdTransform tr;
tr.setFromOpenGLMatrix(m);
GL_ShapeDrawer::DrawCoordSystem();
if (m_simplexSolver)
{
m_simplexSolver->reset();
bool res;
SimdVector3 v;
SimdPoint3 pBuf[4];
SimdPoint3 qBuf[4];
SimdPoint3 yBuf[4];
for (int i=0; i<m_numVertices; i++)
{
v = tr(m_vertices[i]);
m_simplexSolver->addVertex(v,v,SimdPoint3(0.f,0.f,0.f));
res = m_simplexSolver->closest(v);
int res = m_simplexSolver->getSimplex(pBuf, qBuf, yBuf);
}
//draw v?
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(1.f, 0.f, 0.f);
glVertex3f(0.f, 0.f, 0.f);
glVertex3f(v.x(),v.y(),v.z());
glEnd();
glEnable(GL_LIGHTING);
}
}
virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex)
{
for (int i=0;i<3;i++)
{
SimdScalar dot = m_supportVecLocal.dot(triangle[i]);
if (dot > m_maxDot)
{
m_maxDot = dot;
m_supportVertexLocal = triangle[i];
}
}
}
void GLDebugDrawer::DrawContactPoint(const SimdVector3& pointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color)
{
if (m_debugMode & IDebugDraw::DBG_DrawContactPoints)
{
SimdVector3 to=pointOnB+normalOnB*distance;
const SimdVector3&from = pointOnB;
glBegin(GL_LINES);
glColor3f(color.getX(), color.getY(), color.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
glRasterPos3f(from.x(), from.y(), from.z());
char buf[12];
sprintf(buf," %d",lifeTime);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
}
SimdVector3 GetRayTo(int x,int y)
{
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom(eye[0],eye[1],eye[2]);
SimdVector3 rayForward = -rayFrom;
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
SimdVector3 rightOffset;
SimdVector3 vertical(0.f,1.f,0.f);
SimdVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
SimdVector3 rayToCenter = rayFrom + rayForward;
SimdVector3 dHor = hor * 1.f/float(glutScreenWidth);
SimdVector3 dVert = vertical * 1.f/float(glutScreenHeight);
SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
return rayTo;
}
void clientKeyboard(unsigned char key, int x, int y)
{
if ( key == 'R' || key == 'r' )
{
destroyShapes();
g_shapesType[ 0 ] = randomShapeType( 0, 1 );
g_shapesType[ 1 ] = randomShapeType( 0, 1 );
( g_shapesType[ 0 ] == 0 ) ? createBoxShape( 0 ) : createSphereShape( 0 );
( g_shapesType[ 1 ] == 0 ) ? createBoxShape( 1 ) : createSphereShape( 1 );
g_shapesPenetrate = calcPenDepth();
}
else if ( key == 'Q' || key == 'q' )
{
destroyShapes();
}
else if ( key == 'T' || key == 't' )
{
#ifdef DEBUG_ME
SimdVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin();
SimdVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin();
SimdMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis();
SimdMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis();
FILE* fp = 0;
fopen_s( &fp, "shapes.txt", "w" );
char str[ 256 ];
sprintf_s( str, 256, "PosA: %f, %f, %f\nPosB: %f, %f, %f\n", shapeAPos.x(), shapeAPos.y(), shapeAPos.z(),
shapeBPos.x(), shapeBPos.y(), shapeBPos.z() );
fputs( str, fp );
sprintf_s( str, 256, "RotA: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\nRotB: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\n\n",
shapeARot.getRow( 0 ).x(), shapeARot.getRow( 0 ).y(), shapeARot.getRow( 0 ).z(),
shapeARot.getRow( 1 ).x(), shapeARot.getRow( 1 ).y(), shapeARot.getRow( 1 ).z(),
shapeARot.getRow( 2 ).x(), shapeARot.getRow( 2 ).y(), shapeARot.getRow( 2 ).z(),
shapeBRot.getRow( 0 ).x(), shapeBRot.getRow( 0 ).y(), shapeBRot.getRow( 0 ).z(),
shapeBRot.getRow( 1 ).x(), shapeBRot.getRow( 1 ).y(), shapeBRot.getRow( 1 ).z(),
shapeBRot.getRow( 2 ).x(), shapeBRot.getRow( 2 ).y(), shapeBRot.getRow( 2 ).z());
fputs( str, fp );
fclose( fp );
#endif //DEBUG_ME
}
else if ( key == 'P' || key =='p' )
{
g_pauseAnim = !g_pauseAnim;
}
defaultKeyboard(key, x, y);
}
SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
SimdVector3 supVec(0.f,0.f,0.f);
SimdVector3 vec = vec0;
SimdScalar lenSqr = vec.length2();
if (lenSqr < 0.0001f)
{
vec.setValue(1,0,0);
} else
{
float rlen = 1.f / SimdSqrt(lenSqr );
vec *= rlen;
}
LocalSupportVertexCallback supportCallback(vec);
SimdVector3 aabbMax(1e30f,1e30f,1e30f);
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
void AxisSweep3::Quantize(unsigned short* out, const SimdPoint3& point, int isMax) const
{
SimdPoint3 clampedPoint(point);
/*
if (isMax)
clampedPoint += SimdVector3(10,10,10);
else
{
clampedPoint -= SimdVector3(10,10,10);
}
*/
clampedPoint.setMax(m_worldAabbMin);
clampedPoint.setMin(m_worldAabbMax);
SimdVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax);
out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax);
out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax);
}
void PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
//not yet, return box inertia
float margin = GetMargin();
SimdTransform ident;
ident.setIdentity();
SimdVector3 aabbMin,aabbMax;
GetAabb(ident,aabbMin,aabbMax);
SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
SimdScalar lx=2.f*(halfExtents.x()+margin);
SimdScalar ly=2.f*(halfExtents.y()+margin);
SimdScalar lz=2.f*(halfExtents.z()+margin);
const SimdScalar x2 = lx*lx;
const SimdScalar y2 = ly*ly;
const SimdScalar z2 = lz*lz;
const SimdScalar scaledmass = mass * 0.08333333f;
inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
}
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer)
{
if (!m_obsolete) {
if (m_closest.dot(w) < m_dist2) {
edgeBuffer.push_back(ReplaceMeEdge(this, index));
}
else {
m_obsolete = true; // Facet is visible
int next = incMod3(index);
m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
next = incMod3(next);
m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
}
}
}
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;
}
void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
{
RigidBody* body = ctrl->GetRigidBody();
//this m_userPointer is just used for triggers, see CallbackTriggers
body->m_userPointer = ctrl;
body->setGravity( m_gravity );
m_controllers.push_back(ctrl);
m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
assert(body->m_broadphaseHandle);
BroadphaseInterface* scene = GetBroadphase();
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
assert(shapeinterface);
const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
SimdPoint3 minAabb,maxAabb;
shapeinterface->GetAabb(t,minAabb,maxAabb);
float timeStep = 0.02f;
//extent it with the motion
SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
float maxAabbx = maxAabb.getX();
float maxAabby = maxAabb.getY();
float maxAabbz = maxAabb.getZ();
float minAabbx = minAabb.getX();
float minAabby = minAabb.getY();
float minAabbz = minAabb.getZ();
if (linMotion.x() > 0.f)
maxAabbx += linMotion.x();
else
minAabbx += linMotion.x();
if (linMotion.y() > 0.f)
maxAabby += linMotion.y();
else
minAabby += linMotion.y();
if (linMotion.z() > 0.f)
maxAabbz += linMotion.z();
else
minAabbz += linMotion.z();
minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
}
void CcdPhysicsEnvironment::UpdateAabbs(float timeStep)
{
std::vector<CcdPhysicsController*>::iterator i;
BroadphaseInterface* scene = GetBroadphase();
//
// update aabbs, only for moving objects (!)
//
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
SimdPoint3 minAabb,maxAabb;
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
body->getLinearVelocity(),
//body->getAngularVelocity(),
SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
timeStep,minAabb,maxAabb);
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
minAabb -= manifoldExtraExtents;
maxAabb += manifoldExtraExtents;
BroadphaseProxy* bp = body->m_broadphaseHandle;
if (bp)
{
SimdVector3 color (1,1,0);
if (m_debugDrawer)
{
//draw aabb
switch (body->GetActivationState())
{
case ISLAND_SLEEPING:
{
color.setValue(1,1,1);
break;
}
case WANTS_DEACTIVATION:
{
color.setValue(0,0,1);
break;
}
case ACTIVE_TAG:
{
break;
}
case DISABLE_DEACTIVATION:
{
color.setValue(1,0,1);
};
};
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
}
}
if ( (maxAabb-minAabb).length2() < 1e12f)
{
scene->SetAabb(bp,minAabb,maxAabb);
} else
{
//something went wrong, investigate
//removeCcdPhysicsController(ctrl);
body->SetActivationState(DISABLE_SIMULATION);
static bool reportMe = true;
if (reportMe)
{
reportMe = false;
printf("Overflow in AABB, object removed from simulation \n");
printf("If you can reproduce this, please email [email protected]\n");
printf("Please include above information, your Platform, version of OS.\n");
printf("Thanks.\n");
}
}
}
}
}
int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ)
{
CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
ASSERT(rb0);
SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
SimdVector3 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false;
switch (type)
{
case PHY_POINT2POINT_CONSTRAINT:
{
Point2PointConstraint* p2p = 0;
if (rb1)
{
p2p = new Point2PointConstraint(*rb0,
*rb1,pivotInA,pivotInB);
} else
{
p2p = new Point2PointConstraint(*rb0,
pivotInA);
}
m_constraints.push_back(p2p);
p2p->SetUserConstraintId(gConstraintUid++);
p2p->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return p2p->GetUserConstraintId();
break;
}
case PHY_GENERIC_6DOF_CONSTRAINT:
{
Generic6DofConstraint* genericConstraint = 0;
if (rb1)
{
SimdTransform frameInA;
SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
axisInA.y(), axis1.y(), axis2.y(),
axisInA.z(), axis1.z(), axis2.z() );
SimdPlaneSpace1( axisInB, axis1, axis2 );
frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
axisInB.y(), axis1.y(), axis2.y(),
axisInB.z(), axis1.z(), axis2.z() );
frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB );
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
} else
{
// TODO: Implement single body case...
}
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
break;
}
case PHY_ANGULAR_CONSTRAINT:
angularOnly = true;
case PHY_LINEHINGE_CONSTRAINT:
{
HingeConstraint* hinge = 0;
if (rb1)
{
hinge = new HingeConstraint(
*rb0,
*rb1,pivotInA,pivotInB,axisInA,axisInB);
} else
{
hinge = new HingeConstraint(*rb0,
pivotInA,axisInA);
}
hinge->setAngularOnly(angularOnly);
m_constraints.push_back(hinge);
hinge->SetUserConstraintId(gConstraintUid++);
hinge->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return hinge->GetUserConstraintId();
break;
}
#ifdef NEW_BULLET_VEHICLE_SUPPORT
case PHY_VEHICLE_CONSTRAINT:
{
RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
RigidBody* chassis = rb0;
DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
m_wrapperVehicles.push_back(wrapperVehicle);
vehicle->SetUserConstraintId(gConstraintUid++);
vehicle->SetUserConstraintType(type);
return vehicle->GetUserConstraintId();
break;
};
#endif //NEW_BULLET_VEHICLE_SUPPORT
default:
{
}
};
//RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
return 0;
}
void Raytracer::displayCallback()
{
updateCamera();
for (int i=0;i<numObjects;i++)
{
transforms[i].setIdentity();
SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f);
transforms[i].setOrigin( pos );
SimdQuaternion orn;
if (i < 2)
{
orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn);
}
}
myMink.SetTransformA(SimdTransform(transforms[0].getRotation()));
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (once)
{
glGenTextures(1, &glTextureId);
glBindTexture(GL_TEXTURE_2D,glTextureId );
once = 0;
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
#define RAYTRACER
#ifdef RAYTRACER
SimdVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom = getCameraPosition();
SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
SimdVector3 rightOffset;
SimdVector3 vertical(0.f,1.f,0.f);
SimdVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
SimdVector3 rayToCenter = rayFrom + rayForward;
SimdVector3 dHor = hor * 1.f/float(screenWidth);
SimdVector3 dVert = vertical * 1.f/float(screenHeight);
SimdTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
SimdTransform rayFromLocal;
SimdTransform rayToLocal;
SphereShape pointShape(0.0f);
///clear texture
for (int x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
SimdVector4 rgba(0.f,0.f,0.f,0.f);
raytracePicture->SetPixel(x,y,rgba);
}
}
ConvexCast::CastResult rayResult;
SimdTransform rayToTrans;
rayToTrans.setIdentity();
SimdVector3 rayTo;
for (int x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
// rayFromLocal = transforms[s].inverse()* rayFromTrans;
// rayToLocal = transforms[s].inverse()* rayToTrans;
//choose the continuous collision detection method
SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0);
// BU_Simplex1to4 ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex'
// BU_CollisionPair convexCaster(&ptShape,shapePtr[0]);
//reset previous result
rayResult.m_fraction = 1.f;
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
{
//float fog = 1.f - 0.1f * rayResult.m_fraction;
rayResult.m_normal.normalize();
SimdVector3 worldNormal;
worldNormal = transforms[s].getBasis() *rayResult.m_normal;
float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f));
if (light < 0.2f)
light = 0.2f;
if (light > 1.f)
light = 1.f;
rgba = SimdVector4(light,light,light,1.f);
raytracePicture->SetPixel(x,y,rgba);
} else
{
//clear is already done
//rgba = SimdVector4(0.f,0.f,0.f,0.f);
//raytracePicture->SetPixel(x,y,rgba);
}
}
}
}
#define TEST_PRINTF
#ifdef TEST_PRINTF
extern BMF_FontData BMF_font_helv10;
raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10);
char buffer[256];
sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects);
raytracePicture->Printf(buffer,&BMF_font_helv10,0,10);
#endif //TEST_PRINTF
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity(); // Reset The Modelview Matrix
glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,glTextureId );
const unsigned char *ptr = raytracePicture->GetBuffer();
glTexImage2D(GL_TEXTURE_2D,
0,
GL_RGBA,
raytracePicture->GetWidth(),raytracePicture->GetHeight(),
0,
GL_RGBA,
GL_UNSIGNED_BYTE,
ptr);
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f (1,1,1,1); // alpha=0.5=half visible
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2f(-1,1);
glTexCoord2f(1.0f, 0.0f);
glVertex2f(1,1);
glTexCoord2f(1.0f, 1.0f);
glVertex2f(1,-1);
glTexCoord2f(0.0f, 1.0f);
glVertex2f(-1,-1);
glEnd();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
#endif //RAYRACER
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
GL_ShapeDrawer::DrawCoordSystem();
glPushMatrix();
/*
/// normal opengl rendering
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
transA.getOpenGLMatrix( m );
/// draw the simplex
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1));
/// calculate closest point from simplex to the origin, and draw this vector
simplex.CalcClosest(m);
}
*/
glPopMatrix();
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void clientMouseFunc(int button, int state, int x, int y)
{
//printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y);
//button 0, state 0 means left mouse down
SimdVector3 rayTo = GetRayTo(x,y);
switch (button)
{
case 2:
{
if (state==0)
{
shootBox(rayTo);
}
break;
};
case 1:
{
if (state==0)
{
//apply an impulse
if (physicsEnvironmentPtr)
{
float hit[3];
float normal[3];
PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
if (body)
{
body->SetActivationState(ACTIVE_TAG);
SimdVector3 impulse = rayTo;
impulse.normalize();
float impulseStrength = 10.f;
impulse *= impulseStrength;
SimdVector3 relPos(
hit[0] - body->getCenterOfMassPosition().getX(),
hit[1] - body->getCenterOfMassPosition().getY(),
hit[2] - body->getCenterOfMassPosition().getZ());
body->applyImpulse(impulse,relPos);
}
}
}
} else
{
}
break;
}
case 0:
{
if (state==0)
{
//add a point to point constraint for picking
if (physicsEnvironmentPtr)
{
float hit[3];
float normal[3];
PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
if (body)
{
pickedBody = body;
pickedBody->SetActivationState(DISABLE_DEACTIVATION);
SimdVector3 pickPos(hit[0],hit[1],hit[2]);
SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
gPickingConstraintId = physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT,
localPivot.getX(),
localPivot.getY(),
localPivot.getZ(),
0,0,0);
//printf("created constraint %i",gPickingConstraintId);
//save mouse position for dragging
gOldPickingPos = rayTo;
SimdVector3 eyePos(eye[0],eye[1],eye[2]);
gOldPickingDist = (pickPos-eyePos).length();
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//very weak constraint for picking
p2p->m_setting.m_tau = 0.1f;
}
}
}
}
} else
{
if (gPickingConstraintId && physicsEnvironmentPtr)
{
physicsEnvironmentPtr->removeConstraint(gPickingConstraintId);
//printf("removed constraint %i",gPickingConstraintId);
gPickingConstraintId = 0;
pickedBody->ForceActivationState(ACTIVE_TAG);
pickedBody->m_deactivationTime = 0.f;
pickedBody = 0;
}
}
break;
}
default:
{
}
}
}
void LinearConvexCastDemo::displayCallback(void)
{
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
int shapeIndex = 1;
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[shapeIndex].setRotation(orn);
if (m_stepping || m_singleStep)
{
m_singleStep = false;
pitch += 0.005f;
yaw += 0.01f;
}
SimdVector3 fromA(-25,11,0);
SimdVector3 toA(15,11,0);
SimdQuaternion ornFromA(0.f,0.f,0.f,1.f);
SimdQuaternion ornToA(0.f,0.f,0.f,1.f);
SimdTransform rayFromWorld(ornFromA,fromA);
SimdTransform rayToWorld(ornToA,toA);
tr[0] = rayFromWorld;
if (drawLine)
{
glBegin(GL_LINES);
glColor3f(0, 0, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z());
glEnd();
}
//now perform a raycast on the shapes, in local (shape) space
//choose one of the following lines
for (i=1;i<numObjects;i++)
{
ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
//BU_CollisionPair (algebraic version) is currently broken, will look into this
//BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]);
SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
gGjkSimplexSolver.reset();
ConvexCast::CastResult rayResult;
if (convexCaster3.calcTimeOfImpact(rayFromWorld,rayToWorld,tr[i],tr[i],rayResult))
{
glDisable(GL_DEPTH_TEST);
SimdVector3 hitPoint;
hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction);
//draw the raycast result
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(hitPoint.x(),hitPoint.y(),hitPoint.z());
glEnd();
glEnable(GL_DEPTH_TEST);
SimdTransform toTransWorld;
toTransWorld = tr[0];
toTransWorld.setOrigin(hitPoint);
toTransWorld.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,1),getDebugMode());
}
}
glFlush();
glutSwapBuffers();
}
void CollisionWorld::RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans,
CollisionObject* collisionObject,
const CollisionShape* collisionShape,
const SimdTransform& colObjWorldTransform,
RayResultCallback& resultCallback)
{
SphereShape pointShape(0.0f);
if (collisionShape->IsConvex())
{
ConvexCast::CastResult castResult;
castResult.m_fraction = 1.f;//??
ConvexShape* convexShape = (ConvexShape*) collisionShape;
VoronoiSimplexSolver simplexSolver;
SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.length2() > 0.0001f)
{
castResult.m_normal.normalize();
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
CollisionWorld::LocalRayResult localRayResult
(
collisionObject,
0,
castResult.m_normal,
castResult.m_fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
}
else
{
if (collisionShape->IsConcave())
{
TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape;
SimdTransform worldTocollisionObject = colObjWorldTransform.inverse();
SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
//ConvexCast::CastResult
struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback
{
CollisionWorld::RayResultCallback* m_resultCallback;
CollisionObject* m_collisionObject;
TriangleMeshShape* m_triangleMesh;
BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh):
TriangleRaycastCallback(from,to),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
{
}
virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
{
CollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
CollisionWorld::LocalRayResult rayResult
(m_collisionObject,
&shapeInfo,
hitNormalLocal,
hitFraction);
return m_resultCallback->AddSingleResult(rayResult);
}
};
BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
SimdVector3 rayAabbMinLocal = rayFromLocal;
rayAabbMinLocal.setMin(rayToLocal);
SimdVector3 rayAabbMaxLocal = rayFromLocal;
rayAabbMaxLocal.setMax(rayToLocal);
triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
} else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape->IsCompound())
{
const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape);
int i=0;
for (i=0;i<compoundShape->GetNumChildShapes();i++)
{
SimdTransform childTrans = compoundShape->GetChildTransform(i);
const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i);
SimdTransform childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
SimdVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( pInA );
SimdPoint3 qWorld = m_transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" );
if ( delta > 0.f )
return false;
EPA_DEBUG_ASSERT( !simplexSolver.inSimplex( w ) ,"Shapes are disjoint, EPA should have never been called!" );
if (simplexSolver.inSimplex( w ))
return false;
// Add support point to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
bool closestOk = simplexSolver.closest( v );
EPA_DEBUG_ASSERT( closestOk ,"Shapes are disjoint, EPA should have never been called!" );
if (!closestOk)
return false;
SimdScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2();
// Is v converging to v(A-B) ?
EPA_DEBUG_ASSERT( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) ,
"Shapes are disjoint, EPA should have never been called!" );
if (( ( prevVSqrd - squaredDistance ) <= SIMD_EPSILON * prevVSqrd ))
return false;
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
break;
}
++nbIterations;
}
SimdPoint3 simplexPoints[ 5 ];
SimdPoint3 wSupportPointsOnA[ 5 ];
SimdPoint3 wSupportPointsOnB[ 5 ];
int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
// by hybrid penetration depth
EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) ,
"Hybrid Penetration Depth algorithm failed!" );
int nbPolyhedronPoints = nbSimplexPoints;
#ifndef EPA_POLYHEDRON_USE_PLANES
int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
#endif
// Prepare initial polyhedron to start EPA from
if ( nbSimplexPoints == 1 )
{
return false;
}
else if ( nbSimplexPoints == 2 )
{
// We have a line segment inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
d.normalize();
SimdVector3 v1;
SimdVector3 v2;
SimdVector3 v3;
SimdVector3 e1;
SimdScalar absx = abs( d.getX() );
SimdScalar absy = abs( d.getY() );
SimdScalar absz = abs( d.getZ() );
if ( absx < absy )
{
if ( absx < absz )
{
e1.setX( 1 );
}
else
{
e1.setZ( 1 );
}
}
else
{
if ( absy < absz )
{
e1.setY( 1 );
}
else
{
e1.setZ( 1 );
}
}
v1 = d.cross( e1 );
v1.normalize();
v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG );
v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG );
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 2 ] = pWorld;
wSupportPointsOnB[ 2 ] = qWorld;
simplexPoints[ 2 ] = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ];
seperatingAxisInA = v2 * m_transformA.getBasis();
seperatingAxisInB = -v2 * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
seperatingAxisInA = v3 * m_transformA.getBasis();
seperatingAxisInB = -v3 * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 4 ] = pWorld;
wSupportPointsOnB[ 4 ] = qWorld;
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 1 ] = 2;
initTetraIndices[ 2 ] = 3;
initTetraIndices[ 3 ] = 4;
}
else
{
if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 0 ] = 1;
initTetraIndices[ 1 ] = 2;
initTetraIndices[ 2 ] = 3;
initTetraIndices[ 3 ] = 4;
}
else
{
// No tetrahedron contains the origin
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
return false;
}
}
#endif
}
else if ( nbSimplexPoints == 3 )
{
// We have a triangle inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
SimdVector3 triangleNormal = v0.cross( v1 );
triangleNormal.normalize();
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
// We place this check here because if the tetrahedron contains the origin
// there is no need to sample another support point
if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) )
{
#endif
seperatingAxisInA = -triangleNormal * m_transformA.getBasis();
seperatingAxisInB = triangleNormal * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 4 ] = pWorld;
wSupportPointsOnB[ 4 ] = qWorld;
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 3 ] = 4;
}
else
{
// No tetrahedron contains the origin
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
return false;
}
}
#endif
}
#ifdef _DEBUG
else if ( nbSimplexPoints == 4 )
{
EPA_DEBUG_ASSERT( TetrahedronContainsOrigin( simplexPoints ) ,"Initial tetrahedron does not contain the origin!" );
}
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
simplexPoints[ initTetraIndices[ 1 ] ],
simplexPoints[ initTetraIndices[ 2 ] ],
simplexPoints[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
wSupportPointsOnB[ initTetraIndices[ 1 ] ],
wSupportPointsOnB[ initTetraIndices[ 2 ] ],
wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) )
#else
if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) )
#endif
{
// Failed to create initial polyhedron
EPA_DEBUG_ASSERT( false ,"Failed to create initial polyhedron!" );
return false;
}
// Add initial faces to priority queue
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
std::list< EpaFace* >& faces = m_polyhedron.GetFaces();
std::list< EpaFace* >::iterator facesItr( faces.begin() );
while ( facesItr != faces.end() )
{
EpaFace* pFace = *facesItr;
if ( !pFace->m_deleted )
{
//#ifdef EPA_POLYHEDRON_USE_PLANES
// if ( pFace->m_planeDistance >= 0 )
// {
// m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
// assert( false && "Face's plane distance equal or greater than 0!" );
// }
//#endif
if ( pFace->IsAffinelyDependent() )
{
EPA_DEBUG_ASSERT( false ,"One initial face is affinely dependent!" );
return false;
}
if ( pFace->m_vSqrd <= 0 )
{
EPA_DEBUG_ASSERT( false ,"Face containing the origin!" );
return false;
}
if ( pFace->IsClosestPointInternal() )
{
m_faceEntries.push_back( pFace );
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
}
}
++facesItr;
}
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
EPA_DEBUG_ASSERT( !m_faceEntries.empty() ,"No faces added to heap!" );
return true;
}
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
{
SimdVector3 v;
SimdScalar upperBoundSqrd = SIMD_INFINITY;
SimdScalar vSqrd = 0;
#ifdef _DEBUG
SimdScalar prevVSqrd;
#endif
SimdScalar delta;
bool isCloseEnough = false;
EpaFace* pEpaFace = NULL;
int nbIterations = 0;
//int nbMaxIterations = 1000;
do
{
pEpaFace = m_faceEntries.front();
std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
m_faceEntries.pop_back();
if ( !pEpaFace->m_deleted )
{
#ifdef _DEBUG
prevVSqrd = vSqrd;
#endif
vSqrd = pEpaFace->m_vSqrd;
if ( pEpaFace->m_planeDistance >= 0 )
{
v = pEpaFace->m_planeNormal;
}
else
{
v = pEpaFace->m_v;
}
#ifdef _DEBUG
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
EPA_DEBUG_ASSERT( ( vSqrd >= prevVSqrd ) ,"vSqrd decreased!" );
#endif //_DEBUG
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
SimdPoint3 w = pWorld - qWorld;
delta = v.dot( w );
// Keep tighest upper bound
upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
if ( !isCloseEnough )
{
std::list< EpaFace* > newFaces;
bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces );
if ( expandOk )
{
EPA_DEBUG_ASSERT( !newFaces.empty() ,"EPA polyhedron not expanding ?" );
bool check = true;
bool areEqual = false;
while ( !newFaces.empty() )
{
EpaFace* pNewFace = newFaces.front();
EPA_DEBUG_ASSERT( !pNewFace->m_deleted ,"New face is deleted!" );
if ( !pNewFace->m_deleted )
{
EPA_DEBUG_ASSERT( ( pNewFace->m_vSqrd > 0 ) ,"Face containing the origin!" );
EPA_DEBUG_ASSERT( !pNewFace->IsAffinelyDependent() ,"Face is affinely dependent!" );
//#ifdef EPA_POLYHEDRON_USE_PLANES
//// if ( pNewFace->m_planeDistance >= 0 )
//// {
// // assert( false && "Face's plane distance greater than 0!" );
//#ifdef _DEBUG
//// m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" );
//#endif
// //pNewFace->FixOrder();
//#ifdef _DEBUG
// //m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" );
//#endif
//// }
//#endif
//
//#ifdef EPA_POLYHEDRON_USE_PLANES
// //assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" );
//#endif
if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) )
{
m_faceEntries.push_back( pNewFace );
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
}
}
newFaces.pop_front();
}
}
else
{
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
#endif
return v.length();
}
}
}
++nbIterations;
}
while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/
!isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) );
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
#endif
EPA_DEBUG_ASSERT( pEpaFace ,"Invalid epa face!" );
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
return v.length();
}
int main(int argc,char** argv)
{
setCameraDistance(30.f);
#define TRISIZE 10.f
#ifdef DEBUG_MESH
SimdVector3 vert0(-TRISIZE ,0,TRISIZE );
SimdVector3 vert1(TRISIZE ,10,TRISIZE );
SimdVector3 vert2(TRISIZE ,0,-TRISIZE );
meshData.AddTriangle(vert0,vert1,vert2);
SimdVector3 vert3(-TRISIZE ,0,TRISIZE );
SimdVector3 vert4(TRISIZE ,0,-TRISIZE );
SimdVector3 vert5(-TRISIZE ,0,-TRISIZE );
meshData.AddTriangle(vert3,vert4,vert5);
#else
#ifdef ODE_MESH
SimdVector3 Size = SimdVector3(15.f,15.f,12.5f);
gVertices[0][0] = -Size[0];
gVertices[0][1] = Size[2];
gVertices[0][2] = -Size[1];
gVertices[1][0] = Size[0];
gVertices[1][1] = Size[2];
gVertices[1][2] = -Size[1];
gVertices[2][0] = Size[0];
gVertices[2][1] = Size[2];
gVertices[2][2] = Size[1];
gVertices[3][0] = -Size[0];
gVertices[3][1] = Size[2];
gVertices[3][2] = Size[1];
gVertices[4][0] = 0;
gVertices[4][1] = 0;
gVertices[4][2] = 0;
gIndices[0] = 0;
gIndices[1] = 1;
gIndices[2] = 4;
gIndices[3] = 1;
gIndices[4] = 2;
gIndices[5] = 4;
gIndices[6] = 2;
gIndices[7] = 3;
gIndices[8] = 4;
gIndices[9] = 3;
gIndices[10] = 0;
gIndices[11] = 4;
int vertStride = sizeof(SimdVector3);
int indexStride = 3*sizeof(int);
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(NUM_TRIANGLES,
gIndices,
indexStride,
NUM_VERTICES,(float*) &gVertices[0].x(),vertStride);
//shapePtr[4] = new TriangleMeshShape(indexVertexArrays);
shapePtr[4] = new BvhTriangleMeshShape(indexVertexArrays);
#else
int vertStride = sizeof(SimdVector3);
int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 50;
const int NUM_VERTS_Y = 50;
const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
SimdVector3* gVertices = new SimdVector3[totalVerts];
int* gIndices = new int[totalTriangles*3];
int i;
for ( i=0;i<NUM_VERTS_X;i++)
{
for (int j=0;j<NUM_VERTS_Y;j++)
{
gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*10.f,2.f*sinf((float)i)*cosf((float)j),(j-NUM_VERTS_Y*0.5f)*10.f);
}
}
int index=0;
for ( i=0;i<NUM_VERTS_X-1;i++)
{
for (int j=0;j<NUM_VERTS_Y-1;j++)
{
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
}
}
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride);
//shapePtr[4] = new TriangleMeshShape(indexVertexArrays);
shapePtr[4] = new BvhTriangleMeshShape(indexVertexArrays);
#endif
#endif//DEBUG_MESH
// GLDebugDrawer debugDrawer;
//ConstraintSolver* solver = new SimpleConstraintSolver;
ConstraintSolver* solver = new OdeConstraintSolver;
CollisionDispatcher* dispatcher = new CollisionDispatcher();
BroadphaseInterface* broadphase = new SimpleBroadphase();
physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
physicsEnvironmentPtr->setGravity(-1,-10,1);
PHY_ShapeProps shapeProps;
shapeProps.m_do_anisotropic = false;
shapeProps.m_do_fh = false;
shapeProps.m_do_rot_fh = false;
shapeProps.m_friction_scaling[0] = 1.;
shapeProps.m_friction_scaling[1] = 1.;
shapeProps.m_friction_scaling[2] = 1.;
shapeProps.m_inertia = 1.f;
shapeProps.m_lin_drag = 0.95999998f;
shapeProps.m_ang_drag = 0.89999998f;
shapeProps.m_mass = 1.0f;
PHY_MaterialProps materialProps;
materialProps.m_friction = 0.f;// 50.5f;
materialProps.m_restitution = 0.1f;
CcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.f;//50.5f;
ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;
SimdTransform tr;
tr.setIdentity();
for (i=0;i<numObjects;i++)
{
if (i>0)
shapeIndex[i] = 1;//2 = tetrahedron
else
shapeIndex[i] = 4;
}
for (i=0;i<numObjects;i++)
{
shapeProps.m_shape = shapePtr[shapeIndex[i]];
bool isDyna = i>0;
if (!i)
{
//SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI);
//ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
//ms[i].setWorldPosition(0,-10,0);
} else
{
ms[i].setWorldPosition(10,i*15-10,0);
}
//either create a few stacks, to show several islands, or create 1 large stack, showing stability
//ms[i].setWorldPosition((i*5) % 30,i*15-10,0);
ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,0,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
if (!isDyna)
{
shapeProps.m_mass = 0.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
}
else
{
shapeProps.m_mass = 1.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
}
SimdVector3 localInertia;
if (shapeProps.m_mass>0.f)
{
shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
} else
{
localInertia.setValue(0.f,0.f,0.f);
}
ccdObjectCi.m_localInertiaTensor = localInertia;
ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]];
physObjects[i]= new CcdPhysicsController( ccdObjectCi);
physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]);
/* if (i==0)
{
physObjects[i]->SetAngularVelocity(0,0,-2,true);
physObjects[i]->GetRigidBody()->setDamping(0,0);
}
*/
//for the line that represents the AABB extents
// physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
}
return glutmain(argc, argv,640,480,"Static Concave Mesh Demo");
}
//to be implemented by the demo
void renderme()
{
debugDrawer.SetDebugMode(getDebugMode());
//render the hinge axis
if (createConstraint)
{
SimdVector3 color(1,0,0);
SimdVector3 dirLocal(0,1,0);
SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA);
SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB);
SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
debugDrawer.DrawLine(from,from+dirWorldA,color);
debugDrawer.DrawLine(fromB,fromB+dirWorldB,color);
}
float m[16];
int i;
if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP)
{
//don't use Bullet, use quickstep
physicsEnvironmentPtr->setSolverType(0);
} else
{
//Bullet LCP solver
physicsEnvironmentPtr->setSolverType(1);
}
if (getDebugMode() & IDebugDraw::DBG_EnableCCD)
{
physicsEnvironmentPtr->setCcdMode(3);
} else
{
physicsEnvironmentPtr->setCcdMode(0);
}
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);
physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
#ifdef USE_HULL
//some testing code for SAT
if (isSatEnabled)
{
for (int s=0;s<numShapes;s++)
{
CollisionShape* shape = shapePtr[s];
if (shape->IsPolyhedral())
{
PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape);
if (!polyhedron->m_optionalHull)
{
//first convert vertices in 'Point3' format
int numPoints = polyhedron->GetNumVertices();
Point3* points = new Point3[numPoints+1];
//first 4 points should not be co-planar, so add central point to satisfy MakeHull
points[0] = Point3(0.f,0.f,0.f);
SimdVector3 vertex;
for (int p=0;p<numPoints;p++)
{
polyhedron->GetVertex(p,vertex);
points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ());
}
Hull* hull = Hull::MakeHull(numPoints+1,points);
polyhedron->m_optionalHull = hull;
}
}
}
}
#endif //USE_HULL
for (i=0;i<numObjects;i++)
{
SimdTransform transA;
transA.setIdentity();
float pos[3];
float rot[4];
ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);
SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
transA.setRotation(q);
SimdPoint3 dpos;
dpos.setValue(pos[0],pos[1],pos[2]);
transA.setOrigin( dpos );
transA.getOpenGLMatrix( m );
SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1)
{
wireColor = SimdVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
if (i & 1)
{
wireColor += SimdVector3 (1.f,0.f,0.f);
} else
{
wireColor += SimdVector3 (.5f,0.f,0.f);
}
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += SimdVector3 (0.f,1.f, 0.f);
} else
{
wireColor += SimdVector3 (0.f,0.5f,0.f);
}
}
char extraDebug[125];
sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode());
///this block is just experimental code to show some internal issues with replacing shapes on the fly.
if (getDebugMode()!=0 && (i>0))
{
if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]);
//remove the persistent collision pairs that were created based on the previous shape
BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle;
physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
SimdVector3 newinertia;
SimdScalar newmass = 10.f;
physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia);
physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia);
physObjects[i]->GetRigidBody()->updateInertiaTensor();
}
}
}
if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText))
{
float xOffset = 10.f;
float yStart = 20.f;
float yIncr = -2.f;
char buf[124];
glColor3f(0, 0, 0);
#ifdef USE_QUICKPROF
if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
}
#endif //USE_QUICKPROF
//profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT);
//<< std::endl;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"mouse to interact");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"space to reset");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"cursor keys and z,x to navigate");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"i to toggle simulation, s single step");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"q to quit");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"d to toggle deactivation");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"a to draw temporal AABBs");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"h to toggle help text");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP);
bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD);
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"m Bullet GJK = %i",!isSatEnabled);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"n Bullet LCP = %i",useBulletLCP);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
}
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
{
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
switch (num_verts)
{
case 1:
// Touching contact. Yes, we have a collision,
// but no penetration.
return false;
case 2:
{
// We have a line segment inside the Minkowski sum containing the
// origin. Blow it up by adding three additional support points.
SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized();
int axis = dir.furthestAxis();
static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5));
SimdMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
yBuf[2] = pBuf[2] - qBuf[2];
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[1] = pBuf[4];
qBuf[1] = qBuf[4];
yBuf[1] = yBuf[4];
}
else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[0] = pBuf[4];
qBuf[0] = qBuf[4];
yBuf[0] = yBuf[4];
}
else
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
case 3:
{
// We have a triangle inside the Minkowski sum containing
// the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4]))
{
pBuf[3] = pBuf[4];
qBuf[3] = qBuf[4];
yBuf[3] = yBuf[4];
}
else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
}
// We have a tetrahedron inside the Minkowski sum containing
// the origin (if GJK did it's job right ;-)
if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// assert(false);
return false;
}
num_facets = 0;
freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0))
{
return false;
}
f0->link(0, f1, 2);
f0->link(1, f3, 2);
f0->link(2, f2, 0);
f1->link(0, f2, 2);
f1->link(1, f3, 0);
f2->link(1, f3, 1);
if (num_facets == 0)
{
return false;
}
// at least one facet on the heap.
ReplaceMeEdgeBuffer edgeBuffer(20);
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp);
--num_facets;
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
if (num_verts == MaxSupportPoints)
{
#ifdef DEBUG
std::cout << "Ouch, no convergence!!!" << std::endl;
#endif
ASSERT_MESSAGE(false,"Error: pendepth calc failed");
break;
}
pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis()));
qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis()));
yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
{
return false;
}
GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2());
if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2()
#define CHECK_NEW_SUPPORT
#ifdef CHECK_NEW_SUPPORT
|| yBuf[index] == yBuf[(*facet)[0]]
|| yBuf[index] == yBuf[(*facet)[1]]
|| yBuf[index] == yBuf[(*facet)[2]]
#endif
)
{
break;
}
// Compute the silhouette cast by the new vertex
// Note that the new vertex is on the positive side
// of the current facet, so the current facet is will
// not be in the convex hull. Start local search
// from this facet.
facet->silhouette(yBuf[index], edgeBuffer);
if (edgeBuffer.empty())
{
return false;
}
ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin();
ReplaceMeFacet *firstFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!firstFacet)
{
break;
}
firstFacet->link(0, (*it).getFacet(), (*it).getIndex());
ReplaceMeFacet *lastFacet = firstFacet;
++it;
for (; it != edgeBuffer.end(); ++it)
{
ReplaceMeFacet *newFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!newFacet)
{
break;
}
if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex()))
{
break;
}
if (!newFacet->link(2, lastFacet, 1))
{
break;
}
lastFacet = newFacet;
}
if (it != edgeBuffer.end())
{
break;
}
firstFacet->link(2, lastFacet, 1);
}
}
while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2);
#ifdef DEBUG
std::cout << "#facets left = " << num_facets << std::endl;
#endif
v = facet->getClosest();
pa = facet->getClosestPoint(pBuf);
pb = facet->getClosestPoint(qBuf);
return true;
}
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v )
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
{
// Convex shapes do not overlap
// Returning true means that Hybrid's result is ok and there's no need to run EPA
penDepth = 0;
return true;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
//add current vertex to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
//calculate the closest point to the origin (update vector v)
if ( !simplexSolver.closest( v ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
{
simplexSolver.backup_closest( v );
squaredDistance = v.length2();
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
// Convex Shapes intersect - we need to run EPA
// Returning false means that Hybrid couldn't do anything for us
// and that we need to run EPA to calculate the pen depth
return false;
}
++nbIterations;
}
}
bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" );
#endif
if ( nbInitialPoints < 4 )
{
// Insufficient nb of points
return false;
}
////////////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
int nbDiffCoords[ 3 ] = { 0, 0, 0 };
bool* pDiffCoords = new bool[ 3 * nbInitialPoints ];
int i;
for (i=0;i<nbInitialPoints*3;i++)
{
pDiffCoords[i] = false;
}
//::memset( pDiffCoords, 0, sizeof( bool ) * 3 * nbInitialPoints );
int axis;
for ( axis = 0; axis < 3; ++axis )
{
for ( int i = 0; i < nbInitialPoints; ++i )
{
bool isDifferent = true;
for ( int j = 0; j < i; ++j )
{
if ( pInitialPoints[ i ][ axis ] == pInitialPoints[ j ][ axis ] )
{
isDifferent = false;
break;
}
}
if ( isDifferent )
{
++nbDiffCoords[ axis ];
pDiffCoords[ axis * nbInitialPoints + i ] = true;
}
}
if ( nbDiffCoords[ axis ] <= 1 )
{
// The input is degenerate
return false;
}
}
int finalPointsIndices[ 4 ] = { -1, -1, -1, -1 };
int axisOrderIndices[ 3 ] = { 0, 1, 2 };
for ( i = 0; i < 2/*round( nbAxis / 2 )*/; ++i )
{
if ( nbDiffCoords[ i ] > nbDiffCoords[ i + 1 ] )
{
int tmp = nbDiffCoords[ i ];
nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ];
nbDiffCoords[ i + 1 ] = tmp;
tmp = axisOrderIndices[ i ];
axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ];
axisOrderIndices[ i + 1 ] = tmp;
}
}
int nbSuccessfullAxis = 0;
// The axes with less different coordinates choose first
int minsIndices[ 3 ] = { -1, -1, -1 };
int maxsIndices[ 3 ] = { -1, -1, -1 };
int finalPointsIndex = 0;
for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis )
{
int axisIndex = axisOrderIndices[ axis ];
SimdScalar axisMin = SIMD_INFINITY;
SimdScalar axisMax = -SIMD_INFINITY;
for ( int i = 0; i < 4; ++i )
{
// Among the diff coords pick the min and max coords
if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] )
{
if ( pInitialPoints[ i ][ axisIndex ] < axisMin )
{
axisMin = pInitialPoints[ i ][ axisIndex ];
minsIndices[ axisIndex ] = i;
}
if ( pInitialPoints[ i ][ axisIndex ] > axisMax )
{
axisMax = pInitialPoints[ i ][ axisIndex ];
maxsIndices[ axisIndex ] = i;
}
}
}
//assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) &&
// "min and max have the same index!" );
if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) &&
( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) )
{
++nbSuccessfullAxis;
finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ];
finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ];
// Make the choosen points to be impossible for other axes to choose
//assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" );
//assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" );
for ( int i = 0; i < 3; ++i )
{
pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false;
pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false;
}
}
}
if ( nbSuccessfullAxis <= 1 )
{
// Degenerate input ?
assert( false && "nbSuccessfullAxis must be greater than 1!" );
return false;
}
delete[] pDiffCoords;
#endif
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
SimdVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
"Point should be on plane!" );
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
"Point should be on plane!" );
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
"Point should be on plane!" );
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
{
if ( planeDistance > 0 )
{
SimdVector3 tmp = pInitialPoints[ 1 ];
pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
pInitialPoints[ 2 ] = tmp;
tmp = pSupportPointsOnA[ 1 ];
pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ];
pSupportPointsOnA[ 2 ] = tmp;
tmp = pSupportPointsOnB[ 1 ];
pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ];
pSupportPointsOnB[ 2 ] = tmp;
}
}
EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] );
EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] );
EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] );
EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] );
#else
finalPointsIndices[ 3 ] = -1;
SimdScalar absMaxDist = -SIMD_INFINITY;
SimdScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
SimdScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!SimdEqual( dist, PLANE_THICKNESS ) )
{
absMaxDist = absDist;
maxDist = dist;
finalPointsIndices[ 3 ] = pointIndex;
}
}
if ( finalPointsIndices[ 3 ] == -1 )
{
Destroy();
return false;
}
if ( maxDist > PLANE_THICKNESS )
{
// Can swap indices only
SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ];
pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp;
tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ];
pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ];
pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp;
tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ];
pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ];
pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp;
}
EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] );
EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] );
EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] );
EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] );
#endif
EpaFace* pFaceA = CreateFace();
EpaFace* pFaceB = CreateFace();
EpaFace* pFaceC = CreateFace();
EpaFace* pFaceD = CreateFace();
EpaHalfEdge* pFaceAHalfEdges[ 3 ];
EpaHalfEdge* pFaceCHalfEdges[ 3 ];
EpaHalfEdge* pFaceBHalfEdges[ 3 ];
EpaHalfEdge* pFaceDHalfEdges[ 3 ];
pFaceAHalfEdges[ 0 ] = CreateHalfEdge();
pFaceAHalfEdges[ 1 ] = CreateHalfEdge();
pFaceAHalfEdges[ 2 ] = CreateHalfEdge();
pFaceBHalfEdges[ 0 ] = CreateHalfEdge();
pFaceBHalfEdges[ 1 ] = CreateHalfEdge();
pFaceBHalfEdges[ 2 ] = CreateHalfEdge();
pFaceCHalfEdges[ 0 ] = CreateHalfEdge();
pFaceCHalfEdges[ 1 ] = CreateHalfEdge();
pFaceCHalfEdges[ 2 ] = CreateHalfEdge();
pFaceDHalfEdges[ 0 ] = CreateHalfEdge();
pFaceDHalfEdges[ 1 ] = CreateHalfEdge();
pFaceDHalfEdges[ 2 ] = CreateHalfEdge();
pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ];
pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ];
pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ];
pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ];
pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ];
pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ];
pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ];
pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ];
pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ];
pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ];
pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ];
pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ];
pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ];
pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ];
pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA;
pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA;
pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA;
pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB;
pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB;
pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB;
pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC;
pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC;
pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC;
pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD;
pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD;
pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD;
pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA;
pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB;
pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB;
pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD;
pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD;
pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA;
pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB;
pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA;
pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD;
//pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ];
//pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ];
//pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ];
//pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ];
pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ];
pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ];
pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ];
pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ];
pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ];
pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ];
pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ];
pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ];
pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ];
pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ];
pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ];
if ( !pFaceA->Initialize() || !pFaceB->Initialize() ||
!pFaceC->Initialize() || !pFaceD->Initialize() )
{
assert( false && "One initial face failed to initialize!" );
return false;
}
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( nbInitialPoints > 4 )
{
for ( int i = 0; i < nbInitialPoints; ++i )
{
if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) &&
( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) )
{
std::list< EpaFace* >::iterator facesItr( m_faces.begin() );
while ( facesItr != m_faces.end() )
{
EpaFace* pFace = *facesItr;
SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance;
if ( dist > PLANE_THICKNESS )
{
std::list< EpaFace* > newFaces;
bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ],
pFace, newFaces );
if ( !expandOk )
{
// One or more new faces are affinely dependent
return false;
}
assert( !newFaces.empty() && "Polyhedron should have expanded!" );
break;
}
++facesItr;
}
}
}
}
#endif
return true;
}
