public override void GetAabb(ref IndexedMatrix t,out IndexedVector3 aabbMin,out IndexedVector3 aabbMax)
{
float fmargin = GetMargin();
IndexedVector3 margin = new IndexedVector3(fmargin);
aabbMin = t._origin - margin;
aabbMax = t._origin + margin;
}
public static void Initialize(ConvexShape shape0,ref IndexedMatrix wtrs0,
ConvexShape shape1,ref IndexedMatrix wtrs1,
ref GjkEpaSolver2Results results,
GjkEpaSolver2MinkowskiDiff shapeR,
bool withmargins)
{
/* Results */
results.witnesses0 = IndexedVector3.Zero;
results.witnesses1 = IndexedVector3.Zero;
results.status = GjkEpaSolver2Status.Separated;
/* Shape */
shapeR.m_shapes[0] = shape0;
shapeR.m_shapes[1] = shape1;
shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis);
shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
}
#endif
shapeR.EnableMargin(withmargins);
}
public virtual void DrawSphere(float radius, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 start = transform._origin;
IndexedVector3 xoffs = transform._basis * new IndexedVector3(radius, 0, 0);
IndexedVector3 yoffs = transform._basis * new IndexedVector3(0, radius, 0);
IndexedVector3 zoffs = transform._basis * new IndexedVector3(0, 0, radius);
// XY
DrawLine(start - xoffs, start + yoffs, color);
DrawLine(start + yoffs, start + xoffs, color);
DrawLine(start + xoffs, start - yoffs, color);
DrawLine(start - yoffs, start - xoffs, color);
// XZ
DrawLine(start - xoffs, start + zoffs, color);
DrawLine(start + zoffs, start + xoffs, color);
DrawLine(start + xoffs, start - zoffs, color);
DrawLine(start - zoffs, start - xoffs, color);
// YZ
DrawLine(start - yoffs, start + zoffs, color);
DrawLine(start + zoffs, start + yoffs, color);
DrawLine(start + yoffs, start - zoffs, color);
DrawLine(start - zoffs, start - yoffs, color);
}
public DefaultMotionState(IndexedMatrix startTrans, IndexedMatrix centerOfMassOffset)
{
m_graphicsWorldTrans = startTrans;
m_startWorldTrans = startTrans;
m_centerOfMassOffset = centerOfMassOffset;
m_userPointer = null;
}
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;
}
public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
{
m_updateRevision++;
//m_childTransforms.push_back(localTransform);
//m_childShapes.push_back(shape);
CompoundShapeChild child = new CompoundShapeChild();
child.m_transform = localTransform;
child.m_childShape = shape;
child.m_childShapeType = shape.GetShapeType();
child.m_childMargin = shape.GetMargin();
//extend the local aabbMin/aabbMax
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax);
MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);
if (m_dynamicAabbTree != null)
{
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
int index = m_children.Count;
child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
}
m_children.Add(child);
}
protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
{
Generic6DofConstraint d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i = 0; i < 3; i++)
{
if (d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques())
{
IndexedVector3 axis = d6constraint.GetAxis(i);
int tempFlags = ((int)m_flags) >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
SixDofFlags flags = (SixDofFlags)tempFlags;
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm;
}
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
}
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info.erp;
}
row += GetLimitMotorInfo2(d6constraint.GetRotationalLimitMotor(i),
ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 1, false);
}
}
return row;
}
public static bool TestInternalObjects( ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin)
{
float dp = delta_c.Dot(ref axis);
IndexedVector3 localAxis0;
InverseTransformPoint3x3(out localAxis0, ref axis,ref trans0);
IndexedVector3 localAxis1;
InverseTransformPoint3x3(out localAxis1, ref axis,ref trans1);
IndexedVector3 p0;
BoxSupport(ref convex0.m_extents, ref localAxis0, out p0);
IndexedVector3 p1;
BoxSupport(ref convex1.m_extents, ref localAxis1, out p1);
float Radius0 = p0.X*localAxis0.X + p0.Y*localAxis0.Y + p0.Z*localAxis0.Z;
float Radius1 = p1.X*localAxis1.X + p1.Y*localAxis1.Y + p1.Z*localAxis1.Z;
float MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
float MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
float MinMaxRadius = MaxRadius + MinRadius;
float d0 = MinMaxRadius + dp;
float d1 = MinMaxRadius - dp;
float depth = d0<d1 ? d0:d1;
if (depth > dmin)
{
return false;
}
return true;
}
public static bool Distance(ConvexShape shape0,ref IndexedMatrix wtrs0,ConvexShape shape1,ref IndexedMatrix wtrs1,ref IndexedVector3 guess,ref GjkEpaSolver2Results results)
{
using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
using (GJK gjk = BulletGlobals.GJKPool.Get())
{
Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
gjk.Initialise();
GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
if (gjk_status == GJKStatus.Valid)
{
IndexedVector3 w0 = IndexedVector3.Zero;
IndexedVector3 w1 = IndexedVector3.Zero;
for (uint i = 0; i < gjk.m_simplex.rank; ++i)
{
float p = gjk.m_simplex.p[i];
w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
IndexedVector3 temp = -gjk.m_simplex.c[i].d;
w1 += shape.Support(ref temp, 1) * p;
}
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * w1;
results.normal = w0 - w1;
results.distance = results.normal.Length();
results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
return (true);
}
else
{
//GjkEpaSolver2Status
results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed;
return (false);
}
}
}
public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
{
EntityProperties ret = new EntityProperties();
ID = id;
Position = startTransform._origin;
Rotation = startTransform.GetRotation();
return ret;
}
public static void GetPlaneEquationTransformed(StaticPlaneShape plane,ref IndexedMatrix trans, out IndexedVector4 equation)
{
equation = new IndexedVector4();
IndexedVector3 planeNormal = plane.GetPlaneNormal();
equation.X = trans._basis.GetRow(0).Dot(ref planeNormal);
equation.Y = trans._basis.GetRow(1).Dot(ref planeNormal);
equation.Z = trans._basis.GetRow(2).Dot(ref planeNormal);
equation.W = trans._origin.Dot(ref planeNormal) + plane.GetPlaneConstant();
}
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
{
m_frameInA = frameInA;
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameA;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits[0] = new RotationalLimitMotor();
m_angularLimits[1] = new RotationalLimitMotor();
m_angularLimits[2] = new RotationalLimitMotor();
CalculateTransforms();
}
public static void InverseTransformPoint3x3(out IndexedVector3 outVec, ref IndexedVector3 input, ref IndexedMatrix tr)
{
IndexedBasisMatrix rot = tr._basis;
IndexedVector3 r0 = rot._el0;
IndexedVector3 r1 = rot._el1;
IndexedVector3 r2 = rot._el2;
float x = r0.X*input.X + r1.X*input.Y + r2.X*input.Z;
float y = r0.Y*input.X + r1.Y*input.Y + r2.Y*input.Z;
float z = r0.Z*input.X + r1.Z*input.Y + r2.Z*input.Z;
outVec = new IndexedVector3(x, y, z);
}
public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
IndexedMatrix convexFromTrans = convexFromWorld;
IndexedMatrix convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
IndexedVector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
// FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i = 0; i < m_overlappingObjects.Count; i++)
{
CollisionObject collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
IndexedVector3 collisionObjectAabbMin;
IndexedVector3 collisionObjectAabbMax;
IndexedMatrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
IndexedVector3 hitNormal;
if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
{
IndexedMatrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
public virtual void DrawBox(ref IndexedVector3 bbMin, ref IndexedVector3 bbMax, ref IndexedMatrix trans, ref IndexedVector3 color)
{
DrawLine(trans * bbMin, trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * bbMin, color);
DrawLine(trans * bbMin, trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * bbMax, color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), trans * bbMax, color);
DrawLine(trans * bbMax, trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
}
public Generic6DofConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameB)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, GetFixedBody(), rbB)
{
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameB;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits[0] = new RotationalLimitMotor();
m_angularLimits[1] = new RotationalLimitMotor();
m_angularLimits[2] = new RotationalLimitMotor();
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.GetCenterOfMassTransform() * m_frameInB;
CalculateTransforms();
}
public virtual void DrawCapsule(float radius, float halfHeight, int upAxis, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 capStart = IndexedVector3.Zero;;
capStart[upAxis] = -halfHeight;
IndexedVector3 capEnd = IndexedVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
IndexedMatrix childTransform = transform;
childTransform._origin = transform * capStart;
DrawSphere(radius, ref childTransform, ref color);
}
{
IndexedMatrix childTransform = transform;
childTransform._origin = transform * capEnd;
DrawSphere(radius, ref childTransform, ref color);
}
// Draw some additional lines
IndexedVector3 start = transform._origin;
capStart[(upAxis + 1) % 3] = radius;
capEnd[(upAxis + 1) % 3] = radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
capStart[(upAxis + 1) % 3] = -radius;
capEnd[(upAxis + 1) % 3] = -radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
capStart[(upAxis + 2) % 3] = radius;
capEnd[(upAxis + 2) % 3] = radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
capStart[(upAxis + 2) % 3] = -radius;
capEnd[(upAxis + 2) % 3] = -radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
}
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
IndexedVector3 separatingNormal = separatingNormal1.Normalized();
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
float curMaxDist = maxDist;
int closestFaceB = -1;
float dmax = float.MinValue;
{
for (int face = 0; face < hullB.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
// setup initial clip face (minimizing face from hull B)
ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>();
{
Face polyB = hullB.m_faces[closestFaceB];
int numVertices = polyB.m_indices.Count;
for (int e0 = 0; e0 < numVertices; e0++)
{
IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
// check this to see if it is transposed version
worldVertsB1.Add(transB * b);
}
}
if (closestFaceB >= 0)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
}
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
#if true
base.GetAabb(ref t,out aabbMin,out aabbMax);
#else
aabbMin = MathUtil.MAX_VECTOR;
aabbMax = MathUtil.MIN_VECTOR;
//just transform the vertices in worldspace, and take their AABB
for (int i=0;i<m_numVertices;i++)
{
IndexedVector3 worldVertex = IndexedVector3.Transformt(m_vertices[i],t);
MathUtil.vectorMin(ref worldVertex, ref aabbMin);
MathUtil.vectorMin(ref worldVertex,ref aabbMax);
}
#endif
}
public static IndexedVector3 GimInertiaAddTransformed(ref IndexedVector3 source_inertia, ref IndexedVector3 added_inertia, ref IndexedMatrix transform)
{
// btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
// float x2 = transform.getOrigin()[0];
// x2*= x2;
// float y2 = transform.getOrigin()[1];
// y2*= y2;
// float z2 = transform.getOrigin()[2];
// z2*= z2;
// float ix = rotatedTensor[0][0]*(y2+z2);
// float iy = rotatedTensor[1][1]*(x2+z2);
// float iz = rotatedTensor[2][2]*(x2+y2);
// return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
return IndexedVector3.Zero;
}
public virtual void DrawCone(float radius, float height, int upAxis, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 start = transform._origin;
IndexedVector3 offsetHeight = IndexedVector3.Zero;
offsetHeight[upAxis] = height * 0.5f;
IndexedVector3 offsetRadius = IndexedVector3.Zero;
offsetRadius[(upAxis + 1) % 3] = radius;
IndexedVector3 offset2Radius = IndexedVector3.Zero;
offsetRadius[(upAxis + 2) % 3] = radius;
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight + offsetRadius, color);
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight - offsetRadius, color);
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight + offset2Radius, color);
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight - offset2Radius, color);
}
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
///result is conservative
public void CalculateTemporalAabb(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep, out IndexedVector3 temporalAabbMin,out IndexedVector3 temporalAabbMax)
{
//start with static aabb
GetAabb(ref curTrans,out temporalAabbMin,out temporalAabbMax);
float temporalAabbMaxx = temporalAabbMax.X;
float temporalAabbMaxy = temporalAabbMax.Y;
float temporalAabbMaxz = temporalAabbMax.Z;
float temporalAabbMinx = temporalAabbMin.X;
float temporalAabbMiny = temporalAabbMin.Y;
float temporalAabbMinz = temporalAabbMin.Z;
// add linear motion
IndexedVector3 linMotion = linvel*timeStep;
///@todo: simd would have a vector max/min operation, instead of per-element access
if (linMotion.X > 0f)
temporalAabbMaxx += linMotion.X;
else
temporalAabbMinx += linMotion.X;
if (linMotion.Y > 0f)
temporalAabbMaxy += linMotion.Y;
else
temporalAabbMiny += linMotion.Y;
if (linMotion.Z > 0f)
temporalAabbMaxz += linMotion.Z;
else
temporalAabbMinz += linMotion.Z;
//add conservative angular motion
float angularMotion = angvel.Length() * GetAngularMotionDisc() * timeStep;
IndexedVector3 angularMotion3d = new IndexedVector3(angularMotion);
temporalAabbMin = new IndexedVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
temporalAabbMax = new IndexedVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
temporalAabbMin -= angularMotion3d;
temporalAabbMax += angularMotion3d;
}
public static bool TestSepAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 sep_axis, out float depth)
{
float Min0, Max0;
float Min1, Max1;
hullA.Project(ref transA, ref sep_axis, out Min0, out Max0);
hullB.Project(ref transB, ref sep_axis, out Min1, out Max1);
if (Max0 < Min1 || Max1 < Min0)
{
depth = 0;
return false;
}
float d0 = Max0 - Min1;
Debug.Assert(d0 >= 0.0f);
float d1 = Max1 - Min0;
Debug.Assert(d1 >= 0.0f);
depth = d0 < d1 ? d0 : d1;
return true;
}
public virtual void DrawBox(ref IndexedVector3 bbMin, ref IndexedVector3 bbMax, ref IndexedMatrix trans, ref IndexedVector3 color)
{
DrawLine(trans * bbMin, trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * bbMin, color);
DrawLine(trans * bbMin, trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * bbMax, color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), trans * bbMax, color);
DrawLine(trans * bbMax, trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
}
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, out IndexedVector3 sep)
{
gActualSATPairTests++;
// dummy value to satisfy exit points.
sep = new IndexedVector3(0, 1, 0);
#if TEST_INTERNAL_OBJECTS
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
#endif
float dmin = float.MaxValue;
int curPlaneTests = 0;
int numFacesA = hullA.m_faces.Count;
// Test normals from hullA
for (int i = 0; i < numFacesA; i++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
curPlaneTests++;
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(ref transA,ref transB,ref DeltaC2, ref faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
float d;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref faceANormalWS, out d))
return false;
if (d < dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.Count;
// Test normals from hullB
for (int i = 0; i < numFacesB; i++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
curPlaneTests++;
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(ref transA,ref transB,ref DeltaC2, ref WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
float d;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref WorldNormal, out d))
return false;
if (d < dmin)
{
dmin = d;
sep = WorldNormal;
}
}
IndexedVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int curEdgeEdge = 0;
// Test edges
for (int e0 = 0; e0 < hullA.m_uniqueEdges.Count; e0++)
{
IndexedVector3 edge0 = hullA.m_uniqueEdges[e0];
IndexedVector3 WorldEdge0 = transA._basis * edge0;
for (int e1 = 0; e1 < hullB.m_uniqueEdges.Count; e1++)
{
IndexedVector3 edge1 = hullB.m_uniqueEdges[e1];
IndexedVector3 WorldEdge1 = transB._basis * edge1;
IndexedVector3 Cross = IndexedVector3.Cross(WorldEdge0, WorldEdge1);
curEdgeEdge++;
if (!MathUtil.IsAlmostZero(ref Cross))
{
Cross.Normalize();
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(ref transA,ref transB,ref DeltaC2, ref Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
float dist;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref Cross, out dist))
{
return false;
}
if (dist < dmin)
{
dmin = dist;
sep = Cross;
}
}
}
}
IndexedVector3 deltaC = transB._origin - transA._origin;
if ((IndexedVector3.Dot(deltaC, sep)) > 0.0f)
{
sep = -sep;
}
return true;
}
public virtual void DrawPlane(ref IndexedVector3 planeNormal, float planeConst, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 planeOrigin = planeNormal * planeConst;
IndexedVector3 vec0, vec1;
TransformUtil.PlaneSpace1(ref planeNormal, out vec0, out vec1);
float vecLen = 100f;
IndexedVector3 pt0 = planeOrigin + vec0 * vecLen;
IndexedVector3 pt1 = planeOrigin - vec0 * vecLen;
IndexedVector3 pt2 = planeOrigin + vec1 * vecLen;
IndexedVector3 pt3 = planeOrigin - vec1 * vecLen;
DrawLine(transform * pt0, transform * pt1, color);
DrawLine(transform * pt2, transform * pt3, color);
}
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray<IndexedVector3> worldVertsB2 = new ObjectArray<IndexedVector3>();
ObjectArray<IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray<IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
return;
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS=planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray<IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.AddContactPoint(ref separatingNormal,ref point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, out IndexedVector3 sep)
{
return FindSeparatingAxis(hullA, hullB, ref transA, ref transB, out sep);
}
public virtual void DrawSphere(ref IndexedVector3 p, float radius, ref IndexedVector3 color)
{
IndexedMatrix tr = IndexedMatrix.CreateTranslation(p);
DrawSphere(radius, ref tr, ref color);
}
public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut);
}
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform.
public void CalculatePrincipalAxisTransform(IList<float> masses, ref IndexedMatrix principal, out IndexedVector3 inertia)
{
int n = m_children.Count;
float totalMass = 0;
IndexedVector3 center = IndexedVector3.Zero;
for (int k = 0; k < n; k++)
{
Debug.Assert(masses[k] > 0f);
center += m_children[k].m_transform._origin * masses[k];
totalMass += masses[k];
}
Debug.Assert(totalMass > 0f);
center /= totalMass;
principal._origin = center;
IndexedBasisMatrix tensor = new IndexedBasisMatrix();
for (int k = 0; k < n; k++)
{
IndexedVector3 i;
m_children[k].m_childShape.CalculateLocalInertia(masses[k], out i);
IndexedMatrix t = m_children[k].m_transform;
IndexedVector3 o = t._origin - center;
//compute inertia tensor in coordinate system of compound shape
IndexedBasisMatrix j = t._basis.Transpose();
j._el0 *= i.X;
j._el1 *= i.Y;
j._el2 *= i.Z;
j = t._basis * j;
//add inertia tensor
tensor._el0 += j._el0;
tensor._el1 += j._el1;
tensor._el2 += j._el2;
//tensor += j;
//compute inertia tensor of pointmass at o
float o2 = o.LengthSquared();
j._el0 = new IndexedVector3(o2, 0, 0);
j._el1 = new IndexedVector3(0, o2, 0);
j._el2 = new IndexedVector3(0, 0, o2);
j._el0 += o * -o.X;
j._el1 += o * -o.Y;
j._el2 += o * -o.Z;
//add inertia tensor of pointmass
tensor._el0 += masses[k] * j._el0;
tensor._el1 += masses[k] * j._el1;
tensor._el2 += masses[k] * j._el2;
}
tensor.Diagonalize(out principal, 0.00001f, 20);
inertia = new IndexedVector3(tensor._el0.X, tensor._el1.Y, tensor._el2.Z);
}
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 localHalfExtents = .5f * (m_localAabbMax - m_localAabbMin);
IndexedVector3 localCenter = .5f * (m_localAabbMax + m_localAabbMin);
//avoid an illegal AABB when there are no children
if (m_children.Count == 0)
{
localHalfExtents = IndexedVector3.Zero;
localCenter = IndexedVector3.Zero;
}
float margin = GetMargin();
localHalfExtents += new IndexedVector3(margin);
IndexedBasisMatrix abs_b = trans._basis.Absolute();
IndexedVector3 center = trans * localCenter;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref localHalfExtents),
abs_b._el1.Dot(ref localHalfExtents),
abs_b._el2.Dot(ref localHalfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
public void UpdateChildTransform(int childIndex, ref IndexedMatrix newChildTransform, bool shouldRecalculateLocalAabb)
{
m_children[childIndex].m_transform = newChildTransform;
if (m_dynamicAabbTree != null)
{
///update the dynamic aabb tree
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
m_children[childIndex].m_childShape.GetAabb(ref newChildTransform, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//int index = m_children.Count - 1;
m_dynamicAabbTree.Update(m_children[childIndex].m_treeNode, ref bounds);
}
if (shouldRecalculateLocalAabb)
{
RecalculateLocalAabb();
}
}
/**@brief diagonalizes this matrix by the Jacobi method.
* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
* coordinate system, i.e., old_this = rot * new_this * rot^T.
* @param threshold See iteration
* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
* by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
*
* Note that this matrix is assumed to be symmetric.
*/
public void Diagonalize(out IndexedMatrix rot, float threshold, int maxSteps)
{
rot = IndexedMatrix.Identity;
for (int step = maxSteps; step > 0; step--)
{
// find off-diagonal element [p][q] with largest magnitude
int p = 0;
int q = 1;
int r = 2;
float max = Math.Abs(this[0][1]);
float v = Math.Abs(this[0][2]);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = Math.Abs(this[1][2]);
if (v > max)
{
p = 1;
q = 2;
r = 0;
max = v;
}
float t = threshold * (Math.Abs(this[0][0]) + Math.Abs(this[1][1]) + Math.Abs(this[2][2]));
if (max <= t)
{
if (max <= MathUtil.SIMD_EPSILON * t)
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
float mpq = this[p][q];
float theta = (this[q][q] - this[p][p]) / (2 * mpq);
float theta2 = theta * theta;
float cos;
float sin;
if (theta2 * theta2 < (10.0f / MathUtil.SIMD_EPSILON))
{
t = (theta >= 0.0f) ? (1.0f / (float)(theta + (float)Math.Sqrt(1 + theta2)))
: (1 / (theta - (float)Math.Sqrt(1 + theta2)));
cos = 1.0f / (float)Math.Sqrt(1 + t * t);
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + 0.5f / theta2));
cos = 1 - 0.5f * t * t;
sin = cos * t;
}
// apply rotation to matrix (this = J^T * this * J)
this[p, q] = 0;
this[q, p] = 0;
this[p, p] -= t * mpq;
this[q, q] += t * mpq;
float mrp = this[r][p];
float mrq = this[r][q];
this[r, p] = this[p, r] = cos * mrp - sin * mrq;
this[r, q] = this[q, r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for (int i = 0; i < 3; i++)
{
mrp = this[i, p];
mrq = this[i, q];
this[i, p] = cos * mrp - sin * mrq;
this[i, q] = cos * mrq + sin * mrp;
}
}
}
public static void ChPosMatrToBullet(ChVector pos, ChMatrix33 <double> rA, ref BulletXNA.LinearMath.IndexedMatrix mtransform)
{
IndexedBasisMatrix basisA = new IndexedBasisMatrix((float)rA.nm.matrix[0, 0], (float)rA.nm.matrix[0, 1], (float)rA.nm.matrix[0, 2], (float)rA.nm.matrix[1, 0],
(float)rA.nm.matrix[1, 1], (float)rA.nm.matrix[1, 2], (float)rA.nm.matrix[2, 0], (float)rA.nm.matrix[2, 1],
(float)rA.nm.matrix[2, 2]);
mtransform._basis = basisA;
mtransform._origin = new IndexedVector3((float)pos.x, (float)pos.y, (float)pos.z);
}
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
aabbMin = MathUtil.MIN_VECTOR;
aabbMax = MathUtil.MAX_VECTOR;
}
