void renderme()
{
float m[16];
int i;
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(0.f,0.f,1.f); //wants deactivation
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
wireColor = SimdVector3 (1.f,0.f,0.f);
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
wireColor = SimdVector3 (0.f,1.f,0.f);
}
char extraDebug[125];
//sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
shapePtr[shapeIndex[i]]->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[shapeIndex[i]],wireColor,getDebugMode());
}
}
SimdVector3 randomPosition( const SimdPoint3& minPoint, const SimdPoint3& maxPoint )
{
return SimdVector3( randomFloat( minPoint.getX(), maxPoint.getX() ),
randomFloat( minPoint.getY(), maxPoint.getY() ),
randomFloat( minPoint.getZ(), maxPoint.getZ() ) );
}
//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;
}
}
//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;
}
