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 ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
_simplexSolver = simplexSolver;
_penetrationDepthSolver = penetrationDepthSolver;
_convexA = convexA;
_convexB = convexB;
}
public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
_simplexSolver = simplexSolver;
_penetrationDepthSolver = penetrationDepthSolver;
_convexA = convexA;
_convexB = convexB;
}
static CollisionAlgorithm AllocFromPool(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
ConvexConvexAlgorithm result;
if (ObjPool.Count > 0)
result = ObjPool.Dequeue();
else
result = new ConvexConvexAlgorithm();
result.Constructor(mf, ci, body0, body1, simplexSolver, pdSolver, numPerturbationIterations, minimumPointsPerturbationThreshold);
return result;
}
public SimplexTable(TransportTable table, ISimplexSolver solver)
{
_solver = solver;
var simplexTable = FromTransport(table);
Coefficients = simplexTable.Coefficients;
Limitations = simplexTable.Limitations;
Target = simplexTable.Target;
}
//オブジェクトプールを使うので初期化処理の代わりを……
public void Constructor(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
{
base.Constructor(ci);
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
m_ownManifold = false;
m_manifoldPtr = mf;
m_lowLevelOfDetail = false;
m_numPerturbationIterations = numPerturbationIterations;
m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
}
public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,
ISimplexSolver simplexSolver,
IConvexPenetrationDepthSolver penetrationDepthSolver)
{
_cachedSeparatingAxis = new Vector3(0, 0, 1);
_penetrationDepthSolver = penetrationDepthSolver;
_simplexSolver = simplexSolver;
_minkowskiA = objectA;
_minkowskiB = objectB;
_ignoreMargin = false;
_lastUsedMethod = -1;
_catchDegeneracies = 1;
}
public double FunctionValue(ISimplexSolver solver)
{
if (_functionValue != null)
{
return(_functionValue.Value);
}
var simplexTable = new SimplexTable(this, solver);
_minSolution = SolutionToMatrix(simplexTable.Solution());
_functionValue = CalculateFunction(_minSolution);
return(_functionValue.Value);
}
public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,
ISimplexSolver simplexSolver,
IConvexPenetrationDepthSolver penetrationDepthSolver)
{
_cachedSeparatingAxis = new Vector3(0, 0, 1);
_penetrationDepthSolver = penetrationDepthSolver;
_simplexSolver = simplexSolver;
_minkowskiA = objectA;
_minkowskiB = objectB;
_ignoreMargin = false;
_lastUsedMethod = -1;
_catchDegeneracies = 1;
}
public Matrix <double> SolveTable(ISimplexSolver solver)
{
if (_minSolution != null)
{
return(_minSolution);
}
var simplexTable = new SimplexTable(this, solver);
var solution = simplexTable.Solution();
_minSolution = SolutionToMatrix(solution);
_functionValue = CalculateFunction(_minSolution);
return(_minSolution);
}
public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
m_cachedSeparatingAxis = new btVector3(0f, 1f, 0f);
m_penetrationDepthSolver = penetrationDepthSolver;
m_simplexSolver = simplexSolver;
m_minkowskiA = objectA;
m_minkowskiB = objectB;
m_shapeTypeA = objectA.ShapeType;
m_shapeTypeB = objectB.ShapeType;
m_marginA = objectA.Margin;
m_marginB = objectB.Margin;
m_ignoreMargin = false;
m_lastUsedMethod = -1;
m_catchDegeneracies = 1;
m_cachedSeparatingDistance = 0f;
m_curIter = -1;
m_degenerateSimplex = -1;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
{
float radialmargin = 0;
GjkEpaSolver.Results results;
if (GjkEpaSolver.Collide(convexA, transformA,
convexB, transformB,
radialmargin, out results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
ptrA = results.Witnesses[0];
ptrB = results.Witnesses[1];
return true;
}
ptrA = new Vector3();
ptrB = new Vector3();
return false;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
{
float radialmargin = 0;
GjkEpaSolver.Results results;
if (GjkEpaSolver.Collide(convexA, transformA,
convexB, transformB,
radialmargin, out results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
ptrA = results.Witnesses[0];
ptrB = results.Witnesses[1];
return(true);
}
ptrA = new Vector3();
ptrB = new Vector3();
return(false);
}
public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
//_ownsSolvers = false;
_simplexSolver = simplexSolver;
_penetrationDepthSolver = penetrationDepthSolver;
}
public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver pdSolver)
{
m_numPerturbationIterations = 0;
m_minimumPointsPerturbationThreshold = 3;
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
}
public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
{
_simplexSolver = simplexSolver;
_convexA = shapeA;
_convexB = shapeB;
}
public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
//_ownsSolvers = false;
_simplexSolver = simplexSolver;
_penetrationDepthSolver = penetrationDepthSolver;
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
ConvexShape convexA, ConvexShape convexB,
Matrix transformA, Matrix transformB,
Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
{
pa = new Vector3();
pb = new Vector3();
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
Vector3 minNorm = new Vector3();
Vector3 minA = new Vector3(), minB = new Vector3();
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
int numSampleDirections = UnitSpherePointsCount;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
}
{
int numPDA = convexA.PreferredPenetrationDirectionsCount;
if (numPDA != 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformA);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.PreferredPenetrationDirectionsCount;
if (numPDB != 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformB);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = MathHelper.MatrixToVector(transformA, pInA);
qWorld = MathHelper.MatrixToVector(transformB, qInB);
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//add the margins
minA += minNorm * convexA.Margin;
minB -= minNorm * convexB.Margin;
//no penetration
if (minProj < 0)
return false;
minProj += (convexA.Margin + convexB.Margin);
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
Vector3 newOrg = transformA.Translation + offset;
Matrix displacedTrans = transformA;
displacedTrans.Translation = newOrg;
input.TransformA = displacedTrans;
input.TransformB = transformB;
input.MaximumDistanceSquared = 1e30f;//minProj;
IntermediateResult res = new IntermediateResult();
gjkdet.GetClosestPoints(input, res, debugDraw);
float correctedMinNorm = minProj - res.Depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1;
minNorm *= penetration_relaxation;
if (res.HasResult)
{
pa = res.PointInWorld - minNorm * correctedMinNorm;
pb = res.PointInWorld;
}
return res.HasResult;
}
public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
{
_simplexSolver = simplexSolver;
_convexA = shapeA;
_convexB = shapeB;
}
//private bool _ownsSolvers;
public CreateFunc()
{
//_ownsSolvers = true;
_simplexSolver = new VoronoiSimplexSolver();
_penetrationDepthSolver = new GjkEpaPenetrationDepthSolver();
}
public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
ConvexShape convexA, ConvexShape convexB,
Matrix transformA, Matrix transformB,
Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
{
pa = new Vector3();
pb = new Vector3();
//just take fixed number of orientation, and sample the penetration depth in that direction
float minProj = 1e30f;
Vector3 minNorm = new Vector3();
Vector3 minA = new Vector3(), minB = new Vector3();
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
int numSampleDirections = UnitSpherePointsCount;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
}
{
int numPDA = convexA.PreferredPenetrationDirectionsCount;
if (numPDA != 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformA);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.PreferredPenetrationDirectionsCount;
if (numPDB != 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = Vector3.TransformNormal(norm, transformB);
penetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = penetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = MathHelper.MatrixToVector(transformA, pInA);
qWorld = MathHelper.MatrixToVector(transformB, qInB);
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
//add the margins
minA += minNorm * convexA.Margin;
minB -= minNorm * convexB.Margin;
//no penetration
if (minProj < 0)
{
return(false);
}
minProj += (convexA.Margin + convexB.Margin);
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
Vector3 newOrg = transformA.Translation + offset;
Matrix displacedTrans = transformA;
displacedTrans.Translation = newOrg;
input.TransformA = displacedTrans;
input.TransformB = transformB;
input.MaximumDistanceSquared = 1e30f; //minProj;
IntermediateResult res = new IntermediateResult();
gjkdet.GetClosestPoints(input, res, debugDraw);
float correctedMinNorm = minProj - res.Depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1;
minNorm *= penetration_relaxation;
if (res.HasResult)
{
pa = res.PointInWorld - minNorm * correctedMinNorm;
pb = res.PointInWorld;
}
return(res.HasResult);
}
//private bool _ownsSolvers;
public CreateFunc()
{
//_ownsSolvers = true;
_simplexSolver = new VoronoiSimplexSolver();
_penetrationDepthSolver = new GjkEpaPenetrationDepthSolver();
}
public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
