public bool calcPenDepth(ISimplexSolver simplexSolver, ConvexShape pConvexA, ConvexShape pConvexB, btTransform transformA, btTransform transformB, ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB, IDebugDraw debugDraw)
{
btVector3 guessVector;// = transformA.Origin - transformB.Origin;
btVector3.Subtract(ref transformA.Origin,ref transformB.Origin, out guessVector);
GjkEpaSolver2.sResults results = new GjkEpaSolver2.sResults();
if (GjkEpaSolver2.Penetration(pConvexA, transformA,
pConvexB, transformB,
guessVector, ref results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return true;
}
else
{
if (GjkEpaSolver2.Distance(pConvexA, transformA, pConvexB, transformB, guessVector, ref results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return false;
}
}
wWitnessOnA = new btVector3();
wWitnessOnB = new btVector3();
return false;
}
public PerturbedContactResult(/*ManifoldResult* originalResult,*/ btTransform transformA, btTransform transformB, btTransform unPerturbedTransform, bool perturbA, IDebugDraw debugDrawer)
{
//m_originalManifoldResult = originalResult;
m_transformA = transformA;
m_transformB = transformB;
m_unPerturbedTransform = unPerturbedTransform;
m_perturbA = perturbA;
m_debugDrawer = debugDrawer;
}
public CollisionWorld(IDispatcher dispatcher, IBroadphaseInterface pairCache, ICollisionConfiguration collisionConfiguration)
{
m_dispatcher1 = dispatcher;
m_broadphasePairCache = pairCache;
m_debugDrawer = null;
m_forceUpdateAllAabbs = true;
//メモリ確保系?
//m_stackAlloc = collisionConfiguration->getStackAllocator();
//m_dispatchInfo.m_stackAllocator = m_stackAlloc;
}
//メモリ確保系？
//btStackAlloc* m_stackAllocator;
public DispatcherInfo()
{
m_timeStep = 0f;
m_stepCount = 0;
m_dispatchFunc = DispatchFunc.DISPATCH_DISCRETE;
m_timeOfImpact = 1f;
m_useContinuous = false;
m_debugDraw = null;
m_enableSatConvex = false;
m_enableSPU = true;
m_useEpa = true;
m_allowedCcdPenetration = 0.04f;
m_useConvexConservativeDistanceUtil = false;
m_convexConservativeDistanceThreshold = 0.0f;
}
public void getClosestPoints(ref ClosestPointInput input, ref ManifoldResult output, IDebugDraw debugDraw)
{
//float* R1 = stackalloc float[12];
//float* R2 = stackalloc float[12];
StackPtr<float> R1 = StackPtr<float>.Allocate(12);
StackPtr<float> R2 = StackPtr<float>.Allocate(12);
try
{
for (int j = 0; j < 3; j++)
{
R1[0 + 4 * j] = input.m_transformA.Basis[j].X;
R2[0 + 4 * j] = input.m_transformB.Basis[j].X;
R1[1 + 4 * j] = input.m_transformA.Basis[j].Y;
R2[1 + 4 * j] = input.m_transformB.Basis[j].Y;
R1[2 + 4 * j] = input.m_transformA.Basis[j].Z;
R2[2 + 4 * j] = input.m_transformB.Basis[j].Z;
}
btVector3 normal;
float depth;
int return_code;
int maxc = 4;
dBoxBox2(input.m_transformA.Origin,
R1,
m_box1.HalfExtentsWithMargin * 2f,
input.m_transformB.Origin,
R2,
m_box2.HalfExtentsWithMargin * 2f,
out normal, out depth, out return_code,
maxc,
ref output
);
}
finally
{
R1.Dispose();
R2.Dispose();
}
}
public void Constructor(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
IList<TypedConstraint> sortedConstraints,
int numConstraints,
IDebugDraw debugDrawer,
//btStackAlloc* stackAlloc,
IDispatcher dispatcher)
{
m_solverInfo = solverInfo;
m_solver = solver;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
//m_stackAlloc(stackAlloc),
m_dispatcher = dispatcher;
m_bodies.Clear();
m_manifolds.Clear();
m_constraints.Clear();
}
public virtual void allSolved(ContactSolverInfo info, IDebugDraw debugDrawer) { }
//solve a group of constraints public abstract float solveGroup(IList<CollisionObject> bodies, IList<PersistentManifold> manifold, IList<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer, IDispatcher dispatcher);
public abstract void debugDraw(IDebugDraw debugDrawer);
public void getClosestPoints(ref ClosestPointInput input, ref PerturbedContactResult output, ref ManifoldResult originalManifoldResult, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
btVector3 normalInB = new btVector3(0f, 0f, 0f);
btVector3 pointOnA, pointOnB = btVector3.Zero;
btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB;
btVector3 positionOffset;// = (localTransA.Origin + localTransB.Origin) * 0.5f;
{
btVector3 temp;
btVector3.Add(ref localTransA.Origin, ref localTransB.Origin, out temp);
btVector3.Multiply(ref temp, 0.5f, out positionOffset);
}
localTransA.Origin -= positionOffset;
localTransB.Origin -= positionOffset;
bool check2d = m_minkowskiA.isConvex2d && m_minkowskiB.isConvex2d;
float marginA = m_marginA;
float marginB = m_marginB;
//gNumGjkChecks++;
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
{
float squaredDistance = BulletGlobal.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.reset();
for (; ; )
//while (true)
{
btVector3 seperatingAxisInA;// = (-m_cachedSeparatingAxis) * input.m_transformA.Basis;
{
btVector3 temp = -m_cachedSeparatingAxis;
btMatrix3x3.Multiply(ref temp, ref input.m_transformA.Basis, out seperatingAxisInA);
}
btVector3 seperatingAxisInB;// = m_cachedSeparatingAxis * input.m_transformB.Basis;
btMatrix3x3.Multiply(ref m_cachedSeparatingAxis, ref input.m_transformB.Basis, out seperatingAxisInB);
btVector3 pInA;// = m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA, out pInA);
btVector3 qInB;// = m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB, out qInB);
btVector3 pWorld = btVector3.Transform(pInA, localTransA);
btVector3 qWorld = btVector3.Transform(qInB, localTransB);
if (check2d)
{
pWorld.Z = 0f;
qWorld.Z = 0f;
}
btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap
if ((delta > 0.0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver.inSimplex(w))
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
float f0 = squaredDistance - delta;
float f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1)
{
if (f0 <= 0f)
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.addVertex(w, pWorld, qWorld);
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if (!m_simplexSolver.closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.Length2 < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.Length2;
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= BulletGlobal.SIMD_EPSILON * previousSquaredDistance)
{
m_simplexSolver.backup_closest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
/*#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(),
squaredDistance,
m_minkowskiA->getShapeType(),
m_minkowskiB->getShapeType());
#endif */
break;
}
bool check = (!m_simplexSolver.fullSimplex);
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
m_simplexSolver.backup_closest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.compute_points(out pointOnA, out pointOnB);
normalInB = pointOnA - pointOnB;
float lenSqr = m_cachedSeparatingAxis.Length2;
//valid normal
if (lenSqr < 0.0001)
{
m_degenerateSimplex = 5;
}
if (lenSqr > BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON)
{
float rlen = 1f / (float)Math.Sqrt(lenSqr);
normalInB *= rlen; //normalize
float s = (float)Math.Sqrt(squaredDistance);
Debug.Assert(s > 0.0f);
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1f / rlen) - margin);
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies != 0 && m_penetrationDepthSolver != null && m_degenerateSimplex != 0 && ((distance + margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
//penetration case
//if there is no way to handle penetrations, bail out
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
btVector3 tmpPointOnA, tmpPointOnB;
//gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
localTransA, localTransB,
ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw//, input.m_stackAlloc
);
if (isValid2)
{
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.Length2;
if (lenSqr <= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.Length2;
}
if (lenSqr > (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
tmpNormalInB /= (float)Math.Sqrt(lenSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length;
//only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
m_lastUsedMethod = 3;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if (m_cachedSeparatingAxis.Length2 > 0f)
{
float distance2 = (tmpPointOnA - tmpPointOnB).Length - margin;
//only replace valid distances when the distance is less
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA -= m_cachedSeparatingAxis * marginA;
pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
output.addContactPoint(
normalInB,
pointOnB + positionOffset,
distance, ref originalManifoldResult);
}
}