public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = (SphereShape)body0.GetCollisionShape();
SphereShape sphere1 = (SphereShape)body1.GetCollisionShape();
Vector3 diff = body0.GetWorldTransform().Translation - body1.GetWorldTransform().Translation;
float len = diff.Length();
float radius0 = sphere0.GetRadius();
float radius1 = sphere1.GetRadius();
#if CLEAR_MANIFOLD
m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
{
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
float dist = len - (radius0+radius1);
Vector3 normalOnSurfaceB = new Vector3(1,0,0);
if (len > MathUtil.SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
Vector3 pos1 = body1.GetWorldTransform().Translation + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(ref normalOnSurfaceB,ref pos1,dist);
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
}
public ManifoldResult(CollisionObject body0, CollisionObject body1)
{
if (body0 == null || body1 == null)
{
int ibreak = 0;
}
m_body0 = body0;
m_body1 = body1;
m_rootTransA = body0.GetWorldTransform();
m_rootTransB = body1.GetWorldTransform();
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Zero;
Vector3 pointOnBWorld = Vector3.Zero;
{
ClosestPointInput input = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform().Translation - body0.GetWorldTransform().Translation).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform().Translation - body1.GetWorldTransform().Translation).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return resultFraction;
}
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
ConvexShape convex0 = (ConvexShape)(body0.GetCollisionShape());
SphereShape sphere1 = new SphereShape(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast( convex0 ,sphere1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction()> result.m_fraction)
{
body0.SetHitFraction( result.m_fraction );
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = (ConvexShape)(body1.GetCollisionShape());
SphereShape sphere0 = new SphereShape(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(sphere0,convex1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction( result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
return resultFraction;
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
resultOut = new ManifoldResult(body0, body1);
BoxShape box0 = (BoxShape)col0.GetCollisionShape();
BoxShape box1 = (BoxShape)col1.GetCollisionShape();
//if (((String)col0.getUserPointer()).Contains("Box") &&
// ((String)col1.getUserPointer()).Contains("Box") )
//{
// int ibreak = 0;
//}
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.ClearManifold();
#endif //USE_PERSISTENT_CONTACTS
ClosestPointInput input = new ClosestPointInput();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector detector = new BoxBoxDetector(box0,box1);
detector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
public float GetSpherePenetration(CollisionObject boxObj, ref Vector3 v3PointOnBox, ref Vector3 v3PointOnSphere, ref Vector3 v3SphereCenter, float fRadius, ref Vector3 aabbMin, ref Vector3 aabbMax)
{
Vector3[] bounds = new Vector3[2];
bounds[0] = aabbMin;
bounds[1] = aabbMax;
Vector3 p0, tmp, prel, normal;
Vector3[] n = new Vector3[6];
float fSep = -10000000.0f;
float fSepThis;
// set p0 and normal to a default value to shup up GCC
p0 = Vector3.Zero;
normal = Vector3.Zero;
n[0] = new Vector3(-1,0,0);
n[1] = new Vector3(0,-1,0);
n[2] = new Vector3(0,0,-1);
n[3] = new Vector3(1,0,0);
n[4] = new Vector3(0,1,0);
n[5] = new Vector3(0,0,1);
Matrix m44T = boxObj.GetWorldTransform();
// convert point in local space
prel = MathUtil.InverseTransform(ref m44T,ref v3SphereCenter);
///////////
for (int i=0;i<6;i++)
{
int j = i<3 ? 0:1;
if ( (fSepThis = (Vector3.Dot((prel-bounds[j]),n[i]))-fRadius) > 0f)
{
return 1f;
}
if ( fSepThis > fSep )
{
p0 = bounds[j];
normal = n[i];
fSep = fSepThis;
}
}
v3PointOnBox = prel - normal*(Vector3.Dot(normal,(prel-p0)));
v3PointOnSphere = v3PointOnBox + normal*fSep;
// transform back in world space
tmp = Vector3.Transform(v3PointOnBox,m44T );
v3PointOnBox = tmp;
tmp = Vector3.Transform(v3PointOnSphere,m44T);
v3PointOnSphere = tmp;
normal = (v3PointOnBox-v3PointOnSphere);
normal.Normalize();
return fSep;
}
public float GetSphereDistance(CollisionObject boxObj,ref Vector3 v3PointOnBox, ref Vector3 v3PointOnSphere, ref Vector3 v3SphereCenter, float fRadius )
{
float margins;
Vector3[] bounds = new Vector3[2];
BoxShape boxShape= (BoxShape)boxObj.GetCollisionShape();
bounds[0] = -boxShape.GetHalfExtentsWithoutMargin();
bounds[1] = boxShape.GetHalfExtentsWithoutMargin();
margins = boxShape.Margin;//also add sphereShape margin?
Matrix m44T = boxObj.GetWorldTransform();
Vector3[] boundsVec = new Vector3[2];
float fPenetration;
boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1];
Vector3 marginsVec = new Vector3( margins, margins, margins );
// add margins
bounds[0] += marginsVec;
bounds[1] -= marginsVec;
/////////////////////////////////////////////////
Vector3 tmp, prel, normal, v3P;
Vector3[] n = new Vector3[6];
float fSep = 10000000.0f;
float fSepThis;
n[0] = new Vector3(-1,0,0);
n[1] = new Vector3(0,-1,0);
n[2] = new Vector3(0,0,-1);
n[3] = new Vector3(1,0,0);
n[4] = new Vector3(0,1,0);
n[5] = new Vector3(0,0,1);
// convert point in local space
prel = MathUtil.InverseTransform(ref m44T,ref v3SphereCenter);
bool bFound = false;
v3P = prel;
for (int i=0;i<6;i++)
{
int j = i<3? 0:1;
fSepThis = Vector3.Dot((v3P-bounds[j]),n[i]);
if ( fSepThis != 0f)
{
v3P = v3P - n[i]*fSepThis;
bFound = true;
}
}
//
if ( bFound )
{
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
normal = (prel - v3P);
normal.Normalize();
v3PointOnBox = v3P + normal*margins;
v3PointOnSphere = prel - normal*fRadius;
if ( (Vector3.Dot((v3PointOnSphere - v3PointOnBox),normal)) > 0f )
{
return 1f;
}
// transform back in world space
tmp = Vector3.Transform(v3PointOnBox,m44T);
v3PointOnBox = tmp;
tmp = Vector3.Transform(v3PointOnSphere,m44T);
v3PointOnSphere = tmp;
float fSeps2 = (v3PointOnBox-v3PointOnSphere).LengthSquared();
//if this fails, fallback into deeper penetration case, below
if (fSeps2 > MathUtil.SIMD_EPSILON)
{
fSep = (float)-System.Math.Sqrt(fSeps2);
normal = (v3PointOnBox-v3PointOnSphere);
normal *= 1f/fSep;
}
return fSep;
}
//////////////////////////////////////////////////
// Deep penetration case
fPenetration = GetSpherePenetration(boxObj, ref v3PointOnBox, ref v3PointOnSphere, ref v3SphereCenter, fRadius, ref bounds[0], ref bounds[1]);
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
if ( fPenetration <= 0f)
{
return (fPenetration-margins);
}
else
{
return 1f;
}
}
public override void ProcessCollision(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Up;
Vector3 pointOnBWorld = Vector3.Zero;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape) min0;
CapsuleShape capsuleB = (CapsuleShape) min1;
Vector3 localScalingA = capsuleA.GetLocalScaling();
Vector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(ref normalOnB,ref pointOnBWorld,capsuleA.getHalfHeight(),capsuleA.getRadius(),
capsuleB.getHalfHeight(),capsuleB.getRadius(),capsuleA.GetUpAxis(),capsuleB.GetUpAxis(),
body0.GetWorldTransform(),body1.GetWorldTransform(),threshold);
if (dist<threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB,ref pointOnBWorld,dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero;
Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold /radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
Matrix unPerturbedTransform = Matrix.Identity;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i=0;i<m_numPerturbationIterations;i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
//input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.GetWorldTransform());
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA,10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
//input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform());
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB,10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public void UpdateSingleAabb(CollisionObject colObj)
{
Vector3 minAabb = new Vector3();
Vector3 maxAabb = new Vector3();
Matrix wt = colObj.GetWorldTransform();
colObj.GetCollisionShape().GetAabb(ref wt, ref minAabb,ref maxAabb);
//need to increase the aabb for contact thresholds
Vector3 contactThreshold = new Vector3(BulletGlobals.gContactBreakingThreshold, BulletGlobals.gContactBreakingThreshold, BulletGlobals.gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
if (BulletGlobals.g_streamWriter != null && debugCollisionWorld)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb);
}
IBroadphaseInterface bp = (IBroadphaseInterface)m_broadphasePairCache;
//moving objects should be moderately sized, probably something wrong if not
if ( colObj.IsStaticObject() || ((maxAabb-minAabb).LengthSquared() < 1e12f))
{
bp.SetAabb(colObj.GetBroadphaseHandle(),ref minAabb,ref maxAabb, m_dispatcher1);
}
else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj.SetActivationState(ActivationState.DISABLE_SIMULATION);
//static bool reportMe = true;
bool reportMe = true;
if (reportMe && m_debugDrawer != null)
{
reportMe = false;
m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation");
m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n");
m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n");
m_debugDrawer.ReportErrorWarning("Thanks.\n");
}
}
}
public virtual void AddCollisionObject(CollisionObject collisionObject, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
//check that the object isn't already added
//btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
Debug.Assert(collisionObject != null);
m_collisionObjects.Add(collisionObject);
//calculate new AABB
Matrix trans = collisionObject.GetWorldTransform();
Vector3 minAabb = new Vector3();
Vector3 maxAabb= new Vector3();
collisionObject.GetCollisionShape().GetAabb(ref trans,ref minAabb,ref maxAabb);
BroadphaseNativeTypes type = collisionObject.GetCollisionShape().ShapeType;
collisionObject.SetBroadphaseHandle( GetBroadphase().CreateProxy(
ref minAabb,
ref maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask,
m_dispatcher1,0
)) ;
}
protected void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp,
ContactSolverInfo infoGlobal, ref Vector3 vel, ref float rel_vel, ref float relaxation,
ref Vector3 rel_pos1, ref Vector3 rel_pos2)
{
RigidBody rb0 = RigidBody.Upcast(colObj0);
RigidBody rb1 = RigidBody.Upcast(colObj1);
Vector3 pos1 = cp.GetPositionWorldOnA();
Vector3 pos2 = cp.GetPositionWorldOnB();
rel_pos1 = pos1 - colObj0.GetWorldTransform().Translation;
rel_pos2 = pos2 - colObj1.GetWorldTransform().Translation;
relaxation = 1f;
Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 != null ? Vector3.TransformNormal(torqueAxis0, rb0.GetInvInertiaTensorWorld()) * rb0.GetAngularFactor() : Vector3.Zero;
Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.GetNormalWorldOnB());
solverConstraint.m_angularComponentB = rb1 != null ? Vector3.TransformNormal(-torqueAxis1, rb1.GetInvInertiaTensorWorld()) * rb1.GetAngularFactor() : Vector3.Zero;
{
#if COMPUTE_IMPULSE_DENOM
float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
Vector3 vec;
float denom0 = 0f;
float denom1 = 0f;
if (rb0 != null)
{
vec = Vector3.Cross((solverConstraint.m_angularComponentA), rel_pos1);
denom0 = rb0.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec);
}
if (rb1 != null)
{
vec = Vector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2);
denom1 = rb1.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec);
}
#endif //COMPUTE_IMPULSE_DENOM
float denom = relaxation / (denom0 + denom1);
MathUtil.SanityCheckFloat(denom);
solverConstraint.m_jacDiagABInv = denom;
}
solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
solverConstraint.m_relpos1CrossNormal = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB);
solverConstraint.m_relpos2CrossNormal = Vector3.Cross(rel_pos2, -cp.m_normalWorldOnB);
Vector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : Vector3.Zero;
Vector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : Vector3.Zero;
vel = vel1 - vel2;
rel_vel = Vector3.Dot(cp.GetNormalWorldOnB(), vel);
float penetration = cp.GetDistance() + infoGlobal.m_linearSlop;
solverConstraint.m_friction = cp.GetCombinedFriction();
float restitution = 0f;
if (cp.GetLifeTime() > infoGlobal.m_restingContactRestitutionThreshold)
{
restitution = 0f;
}
else
{
restitution = RestitutionCurve(rel_vel, cp.GetCombinedResitution());
if (restitution <= 0f)
{
restitution = 0f;
}
}
///warm starting (or zero if disabled)
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
{
solverConstraint.m_appliedImpulse = cp.GetAppliedImpulse() * infoGlobal.m_warmstartingFactor;
if (rb0 != null)
{
Vector3 contactNormalTemp = solverConstraint.m_contactNormal;
Vector3.Multiply(ref contactNormalTemp, rb0.GetInvMass(), out contactNormalTemp);
rb0.InternalApplyImpulse(ref contactNormalTemp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
}
if (rb1 != null)
{
Vector3 contactNormalTemp = solverConstraint.m_contactNormal;
Vector3.Multiply(ref contactNormalTemp, rb1.GetInvMass(), out contactNormalTemp);
Vector3 negAngular = -solverConstraint.m_angularComponentB;
rb1.InternalApplyImpulse(ref contactNormalTemp, ref negAngular, -solverConstraint.m_appliedImpulse);
}
}
else
{
solverConstraint.m_appliedImpulse = 0f;
}
solverConstraint.m_appliedPushImpulse = 0f;
{
float rel_vel2 = 0f;
float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : Vector3.Zero))
+ Vector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : Vector3.Zero));
float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : Vector3.Zero))
+ Vector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : Vector3.Zero));
rel_vel2 = vel1Dotn + vel2Dotn;
float positionalError = 0f;
positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep;
float velocityError = restitution - rel_vel2;// * damping;
float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_rhsPenetration = 0f;
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
private static void ApplyAnisotropicFriction(CollisionObject colObj, ref Vector3 frictionDirection)
{
if (colObj != null && colObj.HasAnisotropicFriction())
{
// transform to local coordinates
Vector3 loc_lateral = MathUtil.TransposeTransformNormal(frictionDirection, colObj.GetWorldTransform());
Vector3 friction_scaling = colObj.GetAnisotropicFriction();
//apply anisotropic friction
loc_lateral *= friction_scaling;
// ... and transform it back to global coordinates
frictionDirection = Vector3.TransformNormal(loc_lateral, colObj.GetWorldTransform());
}
}
