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; }
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 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 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 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); }