public GjkEpaPenetrationDepthSolver() { } // for pool
public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
{
//float radialmargin = 0f;
IndexedVector3 guessVector = (transA._origin - transB._origin);
GjkEpaSolver2Results results = new GjkEpaSolver2Results();
if (GjkEpaSolver2.Penetration(convexA, ref transA,
convexB, ref transB,
ref 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(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return false;
}
}
return false;
}
///btActionInterface interface
public virtual void DebugDraw(IDebugDraw debugDrawer)
{
for (int v=0;v<GetNumWheels();v++)
{
IndexedVector3 wheelColor = new IndexedVector3(0,1,1);
if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor = new IndexedVector3(0,0,1);
} else
{
wheelColor= new IndexedVector3(1,0,1);
}
IndexedVector3 wheelPosWS = GetWheelInfo(v).m_worldTransform._origin;
IndexedMatrix temp = GetWheelInfo(v).m_worldTransform;
IndexedVector3 axle = new IndexedVector3(
GetWheelInfo(v).m_worldTransform._basis._el0[GetRightAxis()],
GetWheelInfo(v).m_worldTransform._basis._el1[GetRightAxis()],
GetWheelInfo(v).m_worldTransform._basis._el2[GetRightAxis()]);
//debug wheels (cylinders)
debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
}
}
public FrameRateCounter(Game game,Vector3 location,IDebugDraw debugDraw)
: base(game)
{
content = new ContentManager(game.Services);
m_location = location;
m_debugDraw = debugDraw;
}
public bool CalcPenDepth(VoronoiSimplexSolver simplexSolver, ConvexShape convexA,
ConvexShape convexB, Matrix transA, Matrix transB, out Vector3 v, out Vector3 pa,
out Vector3 pb, IDebugDraw debugDraw)
{
return btConvexPenetrationDepthSolver_calcPenDepth(_native, simplexSolver._native,
convexA._native, convexB._native, ref transA, ref transB, out v, out pa,
out pb, DebugDraw.GetUnmanaged(debugDraw));
}
public PerturbedContactResult(ManifoldResult originalResult,ref Matrix transformA,ref Matrix transformB,ref Matrix unPerturbedTransform,bool perturbA,IDebugDraw debugDrawer)
{
m_originalManifoldResult = originalResult;
m_transformA = transformA;
m_transformB = transformB;
m_perturbA = perturbA;
m_unPerturbedTransform = unPerturbedTransform;
m_debugDrawer = debugDrawer;
}
/// <summary>
/// Draws the specified constraint.
/// </summary>
/// <param name="constraint">The constraint.</param>
/// <param name="debugDraw">The debug draw.</param>
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var p6DOF = (Generic6DofConstraint)constraint;
Matrix tr = p6DOF.GetCalculatedTransformA();
if (DrawFrames)
{
debugDraw.DrawTransform(ref tr, DrawSize);
}
tr = p6DOF.GetCalculatedTransformB();
if (DrawFrames)
{
debugDraw.DrawTransform(ref tr, DrawSize);
}
Vector3 zero = Vector3.Zero;
if (DrawLimits)
{
tr = p6DOF.GetCalculatedTransformA();
Vector3 center = p6DOF.GetCalculatedTransformB().Translation;
// up is axis 1 not 2 ?
Vector3 up = MathUtil.MatrixColumn(ref tr, 1);
Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
debugDraw.DrawSpherePatch(ref center, ref up, ref axis, DrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
axis = MathUtil.MatrixColumn(ref tr, 1);
float ay = p6DOF.GetAngle(1);
float az = p6DOF.GetAngle(2);
var cy = (float)System.Math.Cos(ay);
var sy = (float)System.Math.Sin(ay);
var cz = (float)System.Math.Cos(az);
var sz = (float)System.Math.Sin(az);
var ref1 = new Vector3();
ref1.X = cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z;
ref1.Y = -sz * axis.X + cz * axis.Y;
ref1.Z = cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z;
tr = p6DOF.GetCalculatedTransformB();
Vector3 normal = -MathUtil.MatrixColumn(ref tr, 0);
float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
if (minFi > maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI,
ref zero, false);
}
else if (minFi < maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, DrawSize, DrawSize, minFi, maxFi, ref zero, false);
}
tr = p6DOF.GetCalculatedTransformA();
Vector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
Vector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
}
}
//this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
: base(dispatcher, pairCache)
{
_constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver();
_debugDrawer = null;
_gravity = new Vector3(0, -10, 0);
_localTime = 1f / 60f;
_profileTimings = 0;
_islandManager = new SimulationIslandManager();
_ownsIslandManager = true;
_ownsConstraintSolver = constraintSolver == null;
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var pCT = (ConeTwistConstraint)constraint;
Matrix tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
Vector3 zero = Vector3.Zero;
if (DrawLimits)
{
//const float length = float(5);
float length = DrawSize;
int nSegments = 8 * 4;
float fAngleInRadians = MathUtil.SIMD_2_PI * (nSegments - 1) / nSegments;
Vector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
pPrev = Vector3.Transform(pPrev, tr);
for (int i = 0; i < nSegments; i++)
{
fAngleInRadians = MathUtil.SIMD_2_PI * i / nSegments;
Vector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
pCur = Vector3.Transform(pCur, tr);
debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);
if (i % (nSegments / 8) == 0)
{
Vector3 origin = tr.Translation;
debugDraw.DrawLine(ref origin, ref pCur, ref zero);
}
pPrev = pCur;
}
float tws = pCT.GetTwistSpan();
float twa = pCT.GetTwistAngle();
bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
if (useFrameB)
{
tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyB().GetCenterOfMassTransform(), pCT.GetBFrame());
}
else
{
tr = MathUtil.BulletMatrixMultiply(pCT.GetRigidBodyA().GetCenterOfMassTransform(), pCT.GetAFrame());
}
Vector3 pivot = tr.Translation;
Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
Vector3 axis1 = MathUtil.MatrixColumn(ref tr, 1);
debugDraw.DrawArc(ref pivot, ref normal, ref axis1, DrawSize, DrawSize, -twa - tws, -twa + tws, ref zero, true);
}
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var p2pC = (Point2PointConstraint)constraint;
Matrix tr = Matrix.Identity;
Vector3 pivot = p2pC.GetPivotInA();
pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyA().GetCenterOfMassTransform());
tr.Translation = pivot;
debugDraw.DrawTransform(ref tr, DrawSize);
// that ideally should draw the same frame
pivot = p2pC.GetPivotInB();
pivot = Vector3.Transform(pivot, p2pC.GetRigidBodyB().GetCenterOfMassTransform());
tr.Translation = pivot;
if (DrawFrames)
debugDraw.DrawTransform(ref tr, DrawSize);
}
public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
{
Matrix transformA = input.TransformA;
Matrix transformB = input.TransformB;
Vector3 point = new Vector3();
Vector3 normal = new Vector3();
Single timeOfImpact = 1.0f;
Single depth = 0.0f;
//move sphere into triangle space
Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);
if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))
output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);
}
public InplaceSolverIslandCallback(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
ObjectArray<TypedConstraint> sortedConstraints,
int numConstraints,
IDebugDraw debugDrawer,
IDispatcher dispatcher)
{
m_solverInfo = solverInfo;
m_solver = solver;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_dispatcher = dispatcher;
m_bodies = new ObjectArray<CollisionObject>();
m_manifolds = new ObjectArray<PersistentManifold>();
m_constraints = new ObjectArray<TypedConstraint>();
}
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 override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var pSlider = (SliderConstraint)constraint;
Matrix tr = pSlider.GetCalculatedTransformA();
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
tr = pSlider.GetCalculatedTransformB();
if (DrawFrames) debugDraw.DrawTransform(ref tr, DrawSize);
Vector3 zero = Vector3.Zero;
if (DrawLimits)
{
Matrix tr2 = pSlider.GetCalculatedTransformA();
Vector3 li_min = Vector3.Transform(new Vector3(pSlider.GetLowerLinLimit(), 0f, 0f), tr2);
Vector3 li_max = Vector3.Transform(new Vector3(pSlider.GetUpperLinLimit(), 0f, 0f), tr2);
debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
Vector3 normal = MathUtil.MatrixColumn(ref tr, 0);
Vector3 axis = MathUtil.MatrixColumn(ref tr, 1);
float a_min = pSlider.GetLowerAngLimit();
float a_max = pSlider.GetUpperAngLimit();
Vector3 center = pSlider.GetCalculatedTransformB().Translation;
debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, a_min, a_max, ref zero, true);
}
}
internal static IntPtr CreateWrapper(IDebugDraw value, bool weakReference)
{
DrawAabbUnmanagedDelegate a = new DrawAabbUnmanagedDelegate(value.DrawAabb);
/*
_drawArc = new DrawArcUnmanagedDelegate(DrawArc);
_drawBox = new DrawBoxUnmanagedDelegate(DrawBox);
_drawCapsule = new DrawCapsuleUnmanagedDelegate(DrawCapsule);
_drawCone = new DrawConeUnmanagedDelegate(DrawCone);
_drawContactPoint = new DrawContactPointUnmanagedDelegate(DrawContactPoint);
_drawCylinder = new DrawCylinderUnmanagedDelegate(DrawCylinder);
_drawLine = new DrawLineUnmanagedDelegate(DrawLine);
_drawPlane = new DrawPlaneUnmanagedDelegate(DrawPlane);
_drawSphere = new DrawSphereUnmanagedDelegate(DrawSphere);
_drawSpherePatch = new DrawSpherePatchUnmanagedDelegate(DrawSpherePatch);
_drawTransform = new DrawTransformUnmanagedDelegate(DrawTransform);
_drawTriangle = new DrawTriangleUnmanagedDelegate(DrawTriangle);
_getDebugMode = new GetDebugModeUnmanagedDelegate(GetDebugModeUnmanaged);
_cb = new SimpleCallback(SimpleCallbackUnmanaged);
_native = btIDebugDrawWrapper_new(
GCHandle.ToIntPtr(GCHandle.Alloc(this)),
Marshal.GetFunctionPointerForDelegate(_drawAabb),
Marshal.GetFunctionPointerForDelegate(_drawArc),
Marshal.GetFunctionPointerForDelegate(_drawBox),
Marshal.GetFunctionPointerForDelegate(_drawCapsule),
Marshal.GetFunctionPointerForDelegate(_drawCone),
Marshal.GetFunctionPointerForDelegate(_drawContactPoint),
Marshal.GetFunctionPointerForDelegate(_drawCylinder),
Marshal.GetFunctionPointerForDelegate(_drawLine),
Marshal.GetFunctionPointerForDelegate(_drawPlane),
Marshal.GetFunctionPointerForDelegate(_drawSphere),
Marshal.GetFunctionPointerForDelegate(_drawSpherePatch),
Marshal.GetFunctionPointerForDelegate(_drawTransform),
Marshal.GetFunctionPointerForDelegate(_drawTriangle),
Marshal.GetFunctionPointerForDelegate(_getDebugMode),
Marshal.GetFunctionPointerForDelegate(_cb));
*/
return IntPtr.Zero;
}
public override void Draw(TypedConstraint constraint, IDebugDraw debugDraw)
{
var pHinge = (HingeConstraint)constraint;
Matrix tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyA().GetCenterOfMassTransform(), pHinge.GetAFrame());
if (DrawFrames)
debugDraw.DrawTransform(ref tr, DrawSize);
tr = MathUtil.BulletMatrixMultiply(pHinge.GetRigidBodyB().GetCenterOfMassTransform(), pHinge.GetBFrame());
if (DrawFrames)
debugDraw.DrawTransform(ref tr, DrawSize);
float minAng = pHinge.GetLowerLimit();
float maxAng = pHinge.GetUpperLimit();
if (minAng == maxAng)
return;
bool drawSect = true;
if (minAng > maxAng)
{
minAng = 0f;
maxAng = MathUtil.SIMD_2_PI;
drawSect = false;
}
if (DrawLimits)
{
Vector3 center = tr.Translation;
Vector3 normal = MathUtil.MatrixColumn(ref tr, 2);
Vector3 axis = MathUtil.MatrixColumn(ref tr, 0);
Vector3 zero = Vector3.Zero;
debugDraw.DrawArc(ref center, ref normal, ref axis, DrawSize, DrawSize, minAng, maxAng, ref zero, drawSect);
}
}
///btActionInterface interface
public virtual void DebugDraw(IDebugDraw debugDrawer)
{
for (int v=0;v<GetNumWheels();v++)
{
Vector3 wheelColor = new Vector3(0,1,1);
if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor = new Vector3(0,0,1);
} else
{
wheelColor= new Vector3(1,0,1);
}
Vector3 wheelPosWS = GetWheelInfo(v).m_worldTransform.Translation;
Matrix temp = GetWheelInfo(v).m_worldTransform;
Vector3 axle = MathUtil.MatrixColumn(ref temp, GetRightAxis());
//debug wheels (cylinders)
debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
}
}
public void AllSolved(ContactSolverInfo __unnamed0, IDebugDraw __unnamed1)
{
btConstraintSolver_allSolved(Native, __unnamed0.Native, DebugDraw.GetUnmanaged(__unnamed1));
}
public void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw, bool swapResults)
{
btDiscreteCollisionDetectorInterface_getClosestPoints2(_native, input._native, output._native, DebugDraw.GetUnmanaged(debugDraw), swapResults);
}
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;
}
void DrawDot(Body body, IDebugDraw debugDraw)
{
debugDraw.DrawDot(1.75, body.Position, dotColor);
}
internal void InitTarget(IDebugDraw target)
{
_drawAabb = new DrawAabbUnmanagedDelegate(target.DrawAabb);
_drawArc = new DrawArcUnmanagedDelegate(target.DrawArc);
_drawBox = new DrawBoxUnmanagedDelegate(target.DrawBox);
_drawCapsule = new DrawCapsuleUnmanagedDelegate(target.DrawCapsule);
_drawCone = new DrawConeUnmanagedDelegate(target.DrawCone);
_drawContactPoint = new DrawContactPointUnmanagedDelegate(target.DrawContactPoint);
_drawCylinder = new DrawCylinderUnmanagedDelegate(target.DrawCylinder);
_drawLine = new DrawLineUnmanagedDelegate(target.DrawLine);
_drawPlane = new DrawPlaneUnmanagedDelegate(target.DrawPlane);
_drawSphere = new DrawSphereUnmanagedDelegate(target.DrawSphere);
_drawSpherePatch = new DrawSpherePatchUnmanagedDelegate(target.DrawSpherePatch);
_drawTransform = new DrawTransformUnmanagedDelegate(target.DrawTransform);
_drawTriangle = new DrawTriangleUnmanagedDelegate(target.DrawTriangle);
_getDebugMode = new GetDebugModeUnmanagedDelegate(GetDebugModeUnmanaged);
_cb = new SimpleCallback(SimpleCallbackUnmanaged);
_native = btIDebugDrawWrapper_new(
GCHandle.ToIntPtr(GCHandle.Alloc(this)),
Marshal.GetFunctionPointerForDelegate(_drawAabb),
Marshal.GetFunctionPointerForDelegate(_drawArc),
Marshal.GetFunctionPointerForDelegate(_drawBox),
Marshal.GetFunctionPointerForDelegate(_drawCapsule),
Marshal.GetFunctionPointerForDelegate(_drawCone),
Marshal.GetFunctionPointerForDelegate(_drawContactPoint),
Marshal.GetFunctionPointerForDelegate(_drawCylinder),
Marshal.GetFunctionPointerForDelegate(_drawLine),
Marshal.GetFunctionPointerForDelegate(_drawPlane),
Marshal.GetFunctionPointerForDelegate(_drawSphere),
Marshal.GetFunctionPointerForDelegate(_drawSpherePatch),
Marshal.GetFunctionPointerForDelegate(_drawTransform),
Marshal.GetFunctionPointerForDelegate(_drawTriangle),
Marshal.GetFunctionPointerForDelegate(_getDebugMode),
Marshal.GetFunctionPointerForDelegate(_cb));
}
public void Run()
{
using (Graphics = GraphicsLibraryManager.GetGraphics(this))
{
Input = new Input(Graphics.Form);
Freelook = new FreeLook(Input);
Graphics.Initialize();
Graphics.CullingEnabled = isCullingEnabled;
OnInitialize();
if (World == null)
{
OnInitializePhysics();
}
if (_isDebugDrawEnabled)
{
if (_debugDrawer == null)
{
_debugDrawer = Graphics.GetPhysicsDebugDrawer();
_debugDrawer.DebugMode = DebugDrawMode;
}
if (World != null)
{
World.DebugDrawer = _debugDrawer;
}
}
Graphics.UpdateView();
SetInfoText();
Graphics.Run();
if (_debugDrawer != null)
{
if (World != null)
{
World.DebugDrawer = null;
}
if (_debugDrawer is IDisposable)
{
(_debugDrawer as IDisposable).Dispose();
}
_debugDrawer = null;
}
}
Graphics = null;
}
/*
* public void CreateConstraintRows(MultiBodyConstraintArray constraintRows,
* MultiBodyJacobianData data, ContactSolverInfo infoGlobal)
* {
* btMultiBodyConstraint_createConstraintRows(Native, constraintRows.Native,
* data.Native, infoGlobal.Native);
* }
*/
public void DebugDraw(IDebugDraw drawer)
{
btMultiBodyConstraint_debugDraw(Native, BulletSharp.DebugDraw.GetUnmanaged(drawer));
}
public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color)
{
_debugDrawer = debugDrawer;
_worldTrans = worldTrans;
_color = color;
}
public DebugDrawcallback(IDebugDraw debugDrawer, ref IndexedMatrix worldTrans, ref IndexedVector3 color)
{
m_debugDrawer = debugDrawer;
m_color = color;
m_worldTrans = worldTrans;
}
public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
{
StartProfiling(timeStep);
BulletGlobals.StartProfile("stepSimulation");
int numSimulationSubSteps = 0;
if (maxSubSteps != 0)
{
//fixed timestep with interpolation
m_localTime += timeStep;
if (m_localTime >= fixedTimeStep)
{
numSimulationSubSteps = (int)(m_localTime / fixedTimeStep);
m_localTime -= numSimulationSubSteps * fixedTimeStep;
}
}
else
{
//variable timestep
fixedTimeStep = timeStep;
m_localTime = timeStep;
if (MathUtil.FuzzyZero(timeStep))
{
numSimulationSubSteps = 0;
maxSubSteps = 0;
}
else
{
numSimulationSubSteps = 1;
maxSubSteps = 1;
}
}
//process some debugging flags
if (GetDebugDrawer() != null)
{
IDebugDraw debugDrawer = GetDebugDrawer();
BulletGlobals.gDisableDeactivation = ((debugDrawer.GetDebugMode() & DebugDrawModes.DBG_NoDeactivation) != 0);
}
if (numSimulationSubSteps != 0)
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("Stepsimulation numClamped[{0}] timestep[{1:0.00000}]", clampedSimulationSteps, fixedTimeStep));
}
#endif
SaveKinematicState(fixedTimeStep * clampedSimulationSteps);
ApplyGravity();
for (int i = 0; i < clampedSimulationSteps; i++)
{
InternalSingleStepSimulation(fixedTimeStep);
SynchronizeMotionStates();
}
}
else
{
SynchronizeMotionStates();
}
ClearForces();
if (m_profileManager != null)
{
m_profileManager.Increment_Frame_Counter();
}
return(numSimulationSubSteps);
}
} // for pool
public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
{
//float radialmargin = 0f;
IndexedVector3 guessVector = (transA._origin - transB._origin);
GjkEpaSolver2Results results = new GjkEpaSolver2Results();
if (GjkEpaSolver2.Penetration(convexA, ref transA,
convexB, ref transB,
ref 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(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return(false);
}
}
return(false);
}
public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw)
{
bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0;
bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0;
float dbgDrawSize = constraint.GetDbgDrawSize();
if (dbgDrawSize <= 0f)
{
return;
}
switch (constraint.GetConstraintType())
{
case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE:
{
Point2PointConstraint p2pC = constraint as Point2PointConstraint;
IndexedMatrix tr = IndexedMatrix.Identity;
IndexedVector3 pivot = p2pC.GetPivotInA();
pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
debugDraw.DrawTransform(ref tr, dbgDrawSize);
// that ideally should draw the same frame
pivot = p2pC.GetPivotInB();
pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
}
break;
case TypedConstraintType.HINGE_CONSTRAINT_TYPE:
{
HingeConstraint pHinge = constraint as HingeConstraint;
IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
float minAng = pHinge.GetLowerLimit();
float maxAng = pHinge.GetUpperLimit();
if (minAng == maxAng)
{
break;
}
bool drawSect = true;
if (minAng > maxAng)
{
minAng = 0f;
maxAng = MathUtil.SIMD_2_PI;
drawSect = false;
}
if (drawLimits)
{
IndexedVector3 center = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(2);
IndexedVector3 axis = tr._basis.GetColumn(0);
IndexedVector3 zero = IndexedVector3.Zero;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect);
}
}
break;
case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE:
{
ConeTwistConstraint pCT = constraint as ConeTwistConstraint;
IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
//const float length = float(5);
float length = dbgDrawSize;
const int nSegments = 8 * 4;
float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments;
IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i = 0; i < nSegments; i++)
{
fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments;
IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);
if (i % (nSegments / 8) == 0)
{
IndexedVector3 origin = tr._origin;
debugDraw.DrawLine(ref origin, ref pCur, ref zero);
}
pPrev = pCur;
}
float tws = pCT.GetTwistSpan();
float twa = pCT.GetTwistAngle();
bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
if (useFrameB)
{
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
}
else
{
tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
}
IndexedVector3 pivot = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true);
}
}
break;
case TypedConstraintType.D6_CONSTRAINT_TYPE:
case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE:
{
Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint;
IndexedMatrix tr = p6DOF.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = p6DOF.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin;
// up is axis 1 not 2 ?
IndexedVector3 up = tr._basis.GetColumn(1);
IndexedVector3 axis = tr._basis.GetColumn(0);
float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
debugDraw.DrawSpherePatch(ref center, ref up, ref axis, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
axis = tr._basis.GetColumn(1);
float ay = p6DOF.GetAngle(1);
float az = p6DOF.GetAngle(2);
float cy = (float)Math.Cos(ay);
float sy = (float)Math.Sin(ay);
float cz = (float)Math.Cos(az);
float sz = (float)Math.Sin(az);
IndexedVector3 ref1 = new IndexedVector3(
cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z,
-sz * axis.X + cz * axis.Y,
cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z);
tr = p6DOF.GetCalculatedTransformB();
IndexedVector3 normal = -tr._basis.GetColumn(0);
float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
if (minFi > maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false);
}
else if (minFi < maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false);
}
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
}
}
break;
case TypedConstraintType.SLIDER_CONSTRAINT_TYPE:
{
SliderConstraint pSlider = constraint as SliderConstraint;
IndexedMatrix tr = pSlider.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pSlider.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
IndexedMatrix tr2 = pSlider.GetCalculatedTransformA();
IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f);
IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f);
debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
float a_min = pSlider.GetLowerAngLimit();
float a_max = pSlider.GetUpperAngLimit();
IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true);
}
}
break;
default:
break;
}
return;
}
public void Initialise(IDebugDraw debugDrawer, ref IndexedMatrix worldTrans, ref IndexedVector3 color)
{
m_debugDrawer = debugDrawer;
m_color = color;
m_worldTrans = worldTrans;
}
} // for pool
public DebugDrawcallback(IDebugDraw debugDrawer, ref IndexedMatrix worldTrans, ref IndexedVector3 color)
{
m_debugDrawer = debugDrawer;
m_color = color;
m_worldTrans = worldTrans;
}
public void AllSolved(ContactSolverInfo __unnamed0, IDebugDraw __unnamed1)
{
btConstraintSolver_allSolved(_native, __unnamed0._native, DebugDraw.GetUnmanaged(__unnamed1));
}
public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
{
Matrix transformA = input.TransformA;
Matrix transformB = input.TransformB;
Vector3 point = new Vector3();
Vector3 normal = new Vector3();
Single timeOfImpact = 1.0f;
Single depth = 0.0f;
//move sphere into triangle space
Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);
if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))
{
output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);
}
}
public void Draw(IDebugDraw drawer)
{
int i;
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
drawer.DrawLine(ref source[i], ref hit_com[i], ref green);
}
const float normalScale = 10.0f; // easier to see if this is big
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
Vector3 to = hit_surface[i] + normalScale * normal[i];
drawer.DrawLine(ref hit_surface[i], ref to, ref white);
}
Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f);
Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f);
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]);
Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]);
Vector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref from, ref to, 1.0f, out linVel, out angVel);
Matrix T;
TransformUtil.IntegrateTransform(ref from, ref linVel, ref angVel, hit_fraction[i], out T);
Vector3 box1 = boxShapeHalfExtents;
Vector3 box2 = -boxShapeHalfExtents;
drawer.DrawBox(ref box1, ref box2, ref T, ref cyan);
}
}
public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
{
float distance = 0;
Vector3 normalInB = new Vector3();
Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();
Matrix localTransA = input.TransformA;
Matrix localTransB = input.TransformB;
Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;
localTransA.Translation -= positionOffset;
localTransB.Translation -= positionOffset;
float marginA = _minkowskiA.Margin;
float marginB = _minkowskiB.Margin;
_numGjkChecks++;
if (_ignoreMargin)
{
marginA = 0;
marginB = 0;
}
_currentIteration = 0;
int gjkMaxIter = 1000;
_cachedSeparatingAxis = new Vector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
_degenerateSimplex = 0;
_lastUsedMethod = -1;
{
float squaredDistance = MathHelper.Infinity;
float delta = 0;
float margin = marginA + marginB;
_simplexSolver.Reset();
while (true)
{
Matrix transABasis = input.TransformA;
transABasis.Translation = Vector3.Zero;
Matrix transBBasis = input.TransformB;
transBBasis.Translation = Vector3.Zero;
Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);
Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);
Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);
Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);
Vector3 w = pWorld - qWorld;
delta = Vector3.Dot(_cachedSeparatingAxis, w);
if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))
{
checkPenetration = false;
break;
}
if (_simplexSolver.InSimplex(w))
{
_degenerateSimplex = 1;
checkSimplex = true;
break;
}
float f0 = squaredDistance - delta;
float f1 = squaredDistance * RelativeError2;
if (f0 <= f1)
{
if (f0 <= 0.0f)
{
_degenerateSimplex = 2;
}
checkSimplex = true;
break;
}
_simplexSolver.AddVertex(w, pWorld, qWorld);
if (!_simplexSolver.Closest(out _cachedSeparatingAxis))
{
_degenerateSimplex = 3;
checkSimplex = true;
break;
}
float previouseSquaredDistance = squaredDistance;
squaredDistance = _cachedSeparatingAxis.LengthSquared();
if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)
{
_simplexSolver.BackupClosest(out _cachedSeparatingAxis);
checkSimplex = true;
break;
}
if (_currentIteration++ > gjkMaxIter)
{
#if DEBUG
Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);
Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",
_cachedSeparatingAxis.X,
_cachedSeparatingAxis.Y,
_cachedSeparatingAxis.Z,
squaredDistance,
_minkowskiA.ShapeType,
_minkowskiB.ShapeType
);
#endif
break;
}
bool check = (!_simplexSolver.FullSimplex);
if (!check)
{
_simplexSolver.BackupClosest(out _cachedSeparatingAxis);
break;
}
}
if (checkSimplex)
{
_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = pointOnA - pointOnB;
float lenSqr = _cachedSeparatingAxis.LengthSquared();
if (lenSqr < 0.0001f)
{
_degenerateSimplex = 5;
}
if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)
{
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
BulletDebug.Assert(s > 0);
pointOnA -= _cachedSeparatingAxis * (marginA / s);
pointOnB += _cachedSeparatingAxis * (marginB / s);
distance = ((1 / rlen) - margin);
isValid = true;
_lastUsedMethod = 1;
}
else
{
_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
(_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
{
#warning Check this
if (_penetrationDepthSolver != null)
{
Vector3 tmpPointOnA, tmpPointOnB;
_numDeepPenetrationChecks++;
bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(
_simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,
_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw
);
if (isValid2)
{
Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lengSqr = tmpNormalInB.LengthSquared();
if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))
{
tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);
float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
if (!isValid || (distance2 < distance))
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
_lastUsedMethod = 3;
}
else
{
}
}
else
{
_lastUsedMethod = 4;
}
}
else
{
_lastUsedMethod = 5;
}
}
}
if (isValid)
{
output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);
}
}
}
public void DebugDraw(IDebugDraw debugDrawer)
{
for (int v = 0; v < NumWheels; v++)
{
WheelInfo wheelInfo = GetWheelInfo(v);
Color wheelColor;
if (wheelInfo.RaycastInfo.IsInContact)
{
wheelColor = Color.Blue;
}
else
{
wheelColor = Color.Magenta;
}
Matrix transform = wheelInfo.WorldTransform;
Vector3 wheelPosWS = transform.Origin;
Vector3 axle = new Vector3(
transform[0, RightAxis],
transform[1, RightAxis],
transform[2, RightAxis]);
Vector3 to1 = wheelPosWS + axle;
Vector3 to2 = GetWheelInfo(v).RaycastInfo.ContactPointWS;
//debug wheels (cylinders)
debugDrawer.DrawLine(ref wheelPosWS, ref to1, wheelColor);
debugDrawer.DrawLine(ref wheelPosWS, ref to2, wheelColor);
}
}
public void DebugDraw(IDebugDraw debugDrawer)
{
throw new NotImplementedException();
}
public static void DrawNodeTree(SoftBody psb, IDebugDraw iDraw, int minDepth = 0,
int maxDepth = -1)
{
btSoftBodyHelpers_DrawNodeTree(psb.Native, DebugDraw.GetUnmanaged(iDraw),
minDepth, maxDepth);
}
internal DebugDraw(IDebugDraw target)
{
InitTarget(target);
}
public void GetClosestPointsNonVirtual(ClosestPointInput input, Result output,
IDebugDraw debugDraw)
{
btGjkPairDetector_getClosestPointsNonVirtual(_native, input._native,
output._native, DebugDraw.GetUnmanaged(debugDraw));
}
public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
m_cachedSeparatingDistance = 0f;
float distance = 0f;
IndexedVector3 normalInB = IndexedVector3.Zero;
IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero;
IndexedMatrix localTransA = input.m_transformA;
IndexedMatrix localTransB = input.m_transformB;
IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f;
IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin, ref positionOffset);
IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset);
//localTransB._origin -= positionOffset;
bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d();
float marginA = m_marginA;
float marginB = m_marginB;
#if TEST_NON_VIRTUAL
float marginAv = m_minkowskiA.getMarginNonVirtual();
float marginBv = m_minkowskiB.getMarginNonVirtual();
Debug.Assert(marginA == marginAv);
Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL
gNumGjkChecks++;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = 0f;
marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);
}
{
float squaredDistance = MathUtil.BT_LARGE_FLOAT;
float delta = 0f;
float margin = marginA + marginB;
m_simplexSolver.Reset();
int count = 0;
for (; ;)
//while (true)
{
count++;
IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis;
IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis;
IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
IndexedVector3 pWorld = localTransA * pInA;
IndexedVector3 qWorld = localTransB * qInB;
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X, pWorld.Y - qWorld.Y, pWorld.Z - qWorld.Z);
IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w, out delta);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
}
// potential exit, they don't overlap
if ((delta > 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(ref 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(ref w, ref pWorld, ref qWorld);
//calculate the closest point to the origin (update vector v)
IndexedVector3 newCachedSeparatingAxis;
if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
float previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.LengthSquared();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
}
#if false
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance > previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
{
//m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//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.BackupClosest(ref m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if (checkSimplex)
{
m_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
normalInB = m_cachedSeparatingAxis;
float lenSqr = m_cachedSeparatingAxis.LengthSquared();
//valid normal
if (lenSqr < 0.0001f)
{
m_degenerateSimplex = 5;
}
if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
//float rlen = 1 / normalInB.Length();
float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
//normalInB.Normalize();
normalInB *= rlen;
float s = (float)Math.Sqrt((float)squaredDistance);
Debug.Assert(s > 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 && 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 ref
if (m_penetrationDepthSolver != null)
{
// Penetration depth case.
IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis = IndexedVector3.Zero;
bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
debugDraw
);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
}
if (isValid2)
{
IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
float lenSqr = tmpNormalInB.LengthSquared();
if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.LengthSquared();
}
if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.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;
//FIXME! check2d THIS
m_lastUsedMethod = 3;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
//isValid = false;
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.LengthSquared() > 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 (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
{
BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared);
}
if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
IndexedVector3 temp = pointOnB + positionOffset;
output.AddContactPoint(
ref normalInB,
ref temp,
distance);
}
}
internal DebugDraw(IDebugDraw target)
{
InitTarget(target);
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
{
GetClosestPoints(ref input, output, debugDraw, false);
}
public void Draw(IDebugDraw drawer)
{
for (int i = 0; i < NumRays; i++)
{
drawer.DrawLine(ref _source[i], ref _hitCenterOfMass[i], ref _green);
Vector3 to = _hitPoint[i] + NormalScale * _normal[i];
drawer.DrawLine(ref _hitPoint[i], ref to, ref _white);
Matrix fromTransform = _fromRotation * Matrix.Translation(_source[i]);
Matrix toTransform = _toRotation * Matrix.Translation(_destination[i]);
Vector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref fromTransform, ref toTransform, 1.0f, out linVel, out angVel);
Matrix transform;
TransformUtil.IntegrateTransform(ref fromTransform, ref linVel, ref angVel, _hitFraction[i], out transform);
drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, ref _cyan);
}
}
public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
{
GetClosestPointsNonVirtual(ref input, output, debugDraw);
}
public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
{
Vector3 halfExtents = (to - from) * 0.5f;
Vector3 center = (to + from) * 0.5f;
Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
edgecoord.Z * halfExtents.Z);
pa += center;
int othercoord = j % 3;
MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
edgecoord.Z * halfExtents.Z);
pb += center;
debugDrawer.DrawLine(pa, pb, color);
}
edgecoord = new Vector3(-1f, -1f, -1f);
if (i < 3)
MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
}
}
public static void Draw(SoftBody psb, IDebugDraw iDraw, int drawFlags)
{
btSoftBodyHelpers_Draw2(psb._native, DebugDraw.GetUnmanaged(iDraw), drawFlags);
}
public InplaceSolverIslandCallback(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
List<TypedConstraint> sortedConstraints,
IDebugDraw debugDrawer)
{
_solverInfo = solverInfo;
_solver = solver;
_sortedConstraints = sortedConstraints;
_debugDrawer = debugDrawer;
}
///btActionInterface interface
public void DebugDraw(IDebugDraw debugDrawer)
{
}
public void SetDebugDraw(IDebugDraw debugDraw)
{
m_debugDraw = debugDraw;
}
public static void DrawNodeTree(SoftBody psb, IDebugDraw iDraw)
{
btSoftBodyHelpers_DrawNodeTree(psb._native, DebugDraw.GetUnmanaged(iDraw));
}
//public BoxBoxDetector(BoxShape box1, BoxShape box2)
//{
// m_box1 = box1;
// m_box2 = box2;
//}
// Work in progress to copy redo the box detector to remove un-necessary allocations
public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
#endif
int skip = 0;
Object contact = null;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA._origin);
IndexedVector3 translationB = new IndexedVector3(transformB._origin);
//IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());
//IndexedVector3 box1Margin = 2f * debugExtents;
//IndexedVector3 box2Margin = 2f * debugExtents;
IndexedBasisMatrix rotateA = transformA._basis.Transpose();
IndexedBasisMatrix rotateB = transformB._basis.Transpose();
for (int j = 0; j < 3; j++)
{
s_temp1[0 + 4 * j] = transformA._basis[j].X;
s_temp2[0 + 4 * j] = transformB._basis[j].X;
s_temp1[1 + 4 * j] = transformA._basis[j].Y;
s_temp2[1 + 4 * j] = transformB._basis[j].Y;
s_temp1[2 + 4 * j] = transformA._basis[j].Z;
s_temp2[2 + 4 * j] = transformB._basis[j].Z;
}
//s_temp1[0] = rotateA._Row0.X;
//s_temp1[1] = rotateA._Row0.Y;
//s_temp1[2] = rotateA._Row0.Z;
//s_temp1[4] = rotateA._Row1.X;
//s_temp1[5] = rotateA._Row1.Y;
//s_temp1[6] = rotateA._Row1.Z;
//s_temp1[8] = rotateA._Row2.X;
//s_temp1[9] = rotateA._Row2.X;
//s_temp1[10] = rotateA._Row2.X;
//s_temp2[0] = rotateB._Row0.X;
//s_temp2[1] = rotateB._Row0.Y;
//s_temp2[2] = rotateB._Row0.Z;
//s_temp2[4] = rotateB._Row1.X;
//s_temp2[5] = rotateB._Row1.Y;
//s_temp2[6] = rotateB._Row1.Z;
//s_temp2[8] = rotateB._Row2.X;
//s_temp2[9] = rotateB._Row2.Y;
//s_temp2[10] = rotateB._Row2.Z;
DBoxBox2(ref translationA,
s_temp1,
ref box1Margin,
ref translationB,
s_temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
public static void DrawNodeTree(SoftBody psb, IDebugDraw iDraw, int minDepth)
{
btSoftBodyHelpers_DrawNodeTree2(psb._native, DebugDraw.GetUnmanaged(iDraw),
minDepth);
}
public virtual void SetDebugDrawer(IDebugDraw debugDrawer)
{
m_debugDrawer = debugDrawer;
BulletGlobals.gDebugDraw = debugDrawer;
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return(false);
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
IndexedVector3 color = new IndexedVector3(0, 1, 0);
debugDraw.drawLine(minA, minB, color);
color = new IndexedVector3(1, 1, 1);
IndexedVector3 vec = minB - minA;
float prj2 = IndexedVector3.Dot(minNorm, vec);
debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref input, res, debugDraw, false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDraw.drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return(res.m_hasResult);
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();
float minProj = float.MaxValue;
Vector3 minNorm = Vector3.Zero;
Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm = Vector3.Up;
convexA.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm = Vector3.Up;
convexB.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.getNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i=0;i<numPDA;i++)
{
Vector3 norm = Vector3.Zero;
convexA.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i=0;i<numPDB;i++)
{
Vector3 norm = Vector3.Zero;
convexB.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i=0;i<numSampleDirections;i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = Vector3.TransformNormal(-norm, transA);
seperatingAxisInB = Vector3.TransformNormal(norm, transB);
pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return false;
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
Vector3 color = new Vector3(0,1,0);
debugDraw.drawLine(minA,minB,color);
color = new Vector3(1,1,1);
Vector3 vec = minB-minA;
float prj2 = Vector3.Dot(minNorm,vec);
debugDraw.drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
ClosestPointInput input = new ClosestPointInput();
Vector3 newOrg = transA.Translation + offset;
Matrix displacedTrans = transA;
displacedTrans.Translation = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
Vector3 temp = -minNorm;
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(input, res, debugDraw,false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
Vector3 color = new Vector3(1,0,0);
debugDraw.drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
return res.m_hasResult;
}
public static void Draw(SoftBody psb, IDebugDraw iDraw, DrawFlags drawFlags = DrawFlags.Std)
{
btSoftBodyHelpers_Draw(psb.Native, DebugDraw.GetUnmanaged(iDraw), drawFlags);
}
public static void DrawFrame(SoftBody psb, IDebugDraw iDraw)
{
btSoftBodyHelpers_DrawFrame(psb.Native, DebugDraw.GetUnmanaged(iDraw));
}
public static void DrawInfos(SoftBody psb, IDebugDraw iDraw, bool masses,
bool areas, bool stress)
{
btSoftBodyHelpers_DrawInfos(psb.Native, DebugDraw.GetUnmanaged(iDraw),
masses, areas, stress);
}
public static void DrawClusterTree(SoftBody psb, IDebugDraw iDraw, int minDepth,
int maxDepth)
{
btSoftBodyHelpers_DrawClusterTree3(psb._native, DebugDraw.GetUnmanaged(iDraw),
minDepth, maxDepth);
}
//InplaceSolverIslandCallback operator=(InplaceSolverIslandCallback& other)
//{
// Debug.Assert(false);
// //(void)other;
// return *this;
//}
public void Setup(ContactSolverInfo solverInfo, ObjectArray <TypedConstraint> sortedConstraints, int numConstraints, IDebugDraw debugDrawer)
{
Debug.Assert(solverInfo != null);
m_solverInfo = solverInfo;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.Resize(0);
m_manifolds.Resize(0);
m_constraints.Resize(0);
}
