public static IndexedMatrix CreateFromQuaternion(ref IndexedQuaternion q)
{
IndexedMatrix i = new IndexedMatrix();
i._basis.SetRotation(ref q);
return(i);
}
public IndexedVector3 QuatRotate(IndexedQuaternion rotation, IndexedVector3 v)
{
IndexedQuaternion q = rotation * v;
q *= rotation.Inverse();
return(new IndexedVector3(q.X, q.Y, q.Z));
}
public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
{
EntityProperties ret = new EntityProperties();
ID = id;
Position = startTransform._origin;
Rotation = startTransform.GetRotation();
return ret;
}
public IndexedBasisMatrix(ref IndexedQuaternion q)
{
float d = q.LengthSquared();
Debug.Assert(d != 0.0f);
float s = 2.0f / d;
float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
_el0 = new IndexedVector3(1.0f - (yy + zz), xy - wz, xz + wy);
_el1 = new IndexedVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
_el2 = new IndexedVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
}
public void SetRotation(ref IndexedQuaternion q)
{
float d = q.LengthSquared();
Debug.Assert(d != 0.0f);
float s = 2.0f / d;
float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
SetValue(1.0f - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0f - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0f - (xx + yy));
}
public IndexedBasisMatrix(ref IndexedQuaternion q)
{
float d = q.LengthSquared();
Debug.Assert(d != 0.0f);
float s = 2.0f / d;
float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
_el0 = new IndexedVector3(1.0f - (yy + zz), xy - wz, xz + wy);
_el1 = new IndexedVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
_el2 = new IndexedVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
}
public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
{
predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#if QUATERNION_DERIVATIVE
IndexedVector3 pos;
IndexedQuaternion predictedOrn;
IndexedVector3 scale;
curTrans.Decompose(ref scale, ref predictedOrn, ref pos);
predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
predictedOrn.Normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
IndexedVector3 axis;
float fAngle = angvel.Length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
}
IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );
IndexedQuaternion orn0 = curTrans.GetRotation();
IndexedQuaternion predictedOrn = dorn * orn0;
predictedOrn.Normalize();
#endif
IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
predictedTransform._basis = newMatrix._basis;
}
public void SetMotorTarget(ref IndexedQuaternion qAinB, float dt) // qAinB is rotation of body A wrt body B.
{
// convert target from body to constraint space
IndexedQuaternion qConstraint = MathUtil.QuaternionInverse(m_rbBFrame.GetRotation())* qAinB * m_rbAFrame.GetRotation();
qConstraint.Normalize();
// extract "pure" hinge component
IndexedVector3 vNoHinge = MathUtil.QuatRotate(ref qConstraint, ref vHinge);
vNoHinge.Normalize();
IndexedQuaternion qNoHinge = MathUtil.ShortestArcQuat(ref vHinge, ref vNoHinge);
IndexedQuaternion qHinge = MathUtil.QuaternionInverse(ref qNoHinge) * qConstraint;
qHinge.Normalize();
// compute angular target, clamped to limits
float targetAngle = MathUtil.QuatAngle(ref qHinge);
if (targetAngle > MathUtil.SIMD_PI) // long way around. flip quat and recalculate.
{
qHinge = -qHinge;
targetAngle = MathUtil.QuatAngle(ref qHinge);
}
if (qHinge.Z < 0)
{
targetAngle = -targetAngle;
}
SetMotorTarget(targetAngle, dt);
}
public static float Dot(IndexedQuaternion q, IndexedQuaternion q2)
{
return q.X * q2.X + q.Y * q2.Y + q.Z * q2.Z + q.W * q2.W;
}
public static IndexedQuaternion Inverse(IndexedQuaternion q)
{
return new IndexedQuaternion(-q.X, -q.Y, -q.Z, q.W);
}
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
{
//btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
return;
BvhTriangleMeshShape trimesh = null;
if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
}
else
{
trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
}
TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();
if (triangleInfoMapPtr == null)
{
return;
}
int hash = GetHash(partId0, index0);
TriangleInfo info;
if (!triangleInfoMapPtr.TryGetValue(hash, out info))
{
return;
}
float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;
TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape;
IndexedVector3 v0, v1, v2;
tri_shape.GetVertex(0, out v0);
tri_shape.GetVertex(1, out v1);
tri_shape.GetVertex(2, out v2);
IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);
IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
IndexedVector3 tri_normal;
tri_shape.CalcNormal(out tri_normal);
//float dot = tri_normal.dot(cp.m_normalWorldOnB);
IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
IndexedVector3 contact = cp.m_localPointB;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedMatrix tr = colObj0.GetWorldTransform();
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.Normalize();//is this necessary?
// Get closest edge
int bestedge = -1;
float disttobestedge = MathUtil.BT_LARGE_FLOAT;
//
// Edge 0 . 1
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 0;
disttobestedge = len;
}
}
// Edge 1 . 2
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 1;
disttobestedge = len;
}
}
// Edge 2 . 0
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 2;
disttobestedge = len;
}
}
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 0)
{
IndexedVector3 edge = (v0 - v1);
isNearEdge = true;
if (info.m_edgeV0V1Angle == 0.0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 1)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v1 - v2);
isNearEdge = true;
if (info.m_edgeV1V2Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 2)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v2 - v0);
if (info.m_edgeV2V0Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
#if DEBUG_INTERNAL_EDGE
{
IndexedVector3 color = new IndexedVector3(0, 1, 1);
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits > 0)
{
if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.Dot(ref localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * tri_normal;
}
else
{
IndexedVector3 newNormal = tri_normal * frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
float d = newNormal.Dot(ref localContactNormalOnB);
if (d < 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
public virtual void Rotate(IndexedQuaternion iq)
{
IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(iq);
im._origin = m_graphicsWorldTrans._origin;
SetWorldTransform(ref im);
}
internal static bool AlmostEqual(ref IndexedQuaternion v1, ref IndexedQuaternion v2, float nEpsilon)
{
return
(((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
(((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
(((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) &&
(((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon)));
}
public void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB)
{
IndexedVector3 toPosA = transA._origin;
IndexedVector3 toPosB = transB._origin;
IndexedQuaternion toOrnA = transA.GetRotation();
IndexedQuaternion toOrnB = transB.GetRotation();
if (m_separatingDistance > 0.0f)
{
IndexedVector3 linVelA;
IndexedVector3 angVelA;
IndexedVector3 linVelB;
IndexedVector3 angVelB;
TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA);
TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB);
float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
IndexedVector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal);
if (relLinVelocLength < 0f)
{
relLinVelocLength = 0f;
}
float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB)
{
m_separatingNormal = separatingVector;
m_separatingDistance = separatingDistance;
IndexedVector3 toPosA = transA._origin;
IndexedVector3 toPosB = transB._origin;
IndexedQuaternion toOrnA = transA.GetRotation();
IndexedQuaternion toOrnB = transB.GetRotation();
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
public static void GetRotation(ref IndexedBasisMatrix a, out IndexedQuaternion rot)
{
rot = a.GetRotation();
}
public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle)
{
IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a);
IndexedQuaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.Normalize();
angle = MathUtil.QuatAngle(ref dorn);
axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);
//check for axis length
float len = axis.LengthSquared();
if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
axis = new IndexedVector3(1f, 0, 0);
}
else
{
axis.Normalize();
}
}
public static void CalculateVelocityQuaternion(ref IndexedVector3 pos0, ref IndexedVector3 pos1, ref IndexedQuaternion orn0, ref IndexedQuaternion orn1, float timeStep, out IndexedVector3 linVel, out IndexedVector3 angVel)
{
linVel = (pos1 - pos0) / timeStep;
if (orn0 != orn1)
{
IndexedVector3 axis;
float angle;
CalculateDiffAxisAngleQuaternion(ref orn0, ref orn1, out axis, out angle);
angVel = axis * (angle / timeStep);
}
else
{
angVel = IndexedVector3.Zero;
}
}
//SetFrames2(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
internal static void SetFrames2(object pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
{
Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
frame2._origin = frame1v;
constraint.SetFrames(ref frame1, ref frame2);
}
public static IndexedQuaternion Inverse(IndexedQuaternion q)
{
return(new IndexedQuaternion(-q.X, -q.Y, -q.Z, q.W));
}
public override void SetTranslation(BulletBody pBody, Vector3 _position, Quaternion _orientation)
{
RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
_orientation.W);
IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
mat._origin = vposition;
body.SetWorldTransform(mat);
}
public float Dot(IndexedQuaternion q)
{
return(X * q.X + Y * q.Y + Z * q.Z + W * q.W);
}
public static bool ClampNormal(ref IndexedVector3 edge, ref IndexedVector3 tri_normal_org, ref IndexedVector3 localContactNormalOnB, float correctedEdgeAngle, out IndexedVector3 clampedLocalNormal)
{
IndexedVector3 tri_normal = tri_normal_org;
//we only have a local triangle normal, not a local contact normal . only normal in world space...
//either compute the current angle all in local space, or all in world space
IndexedVector3 edgeCross = IndexedVector3.Cross(edge, tri_normal).Normalized();
float curAngle = GetAngle(ref edgeCross, ref tri_normal, ref localContactNormalOnB);
if (correctedEdgeAngle < 0)
{
if (curAngle < correctedEdgeAngle)
{
float diffAngle = correctedEdgeAngle - curAngle;
IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle);
clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB;
return true;
}
}
if (correctedEdgeAngle >= 0)
{
if (curAngle > correctedEdgeAngle)
{
float diffAngle = correctedEdgeAngle - curAngle;
IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle);
clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB;
return true;
}
}
clampedLocalNormal = IndexedVector3.Zero;
return false;
}
public static float Dot(IndexedQuaternion q, IndexedQuaternion q2)
{
return(q.X * q2.X + q.Y * q2.Y + q.Z * q2.Z + q.W * q2.W);
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
//skip self-collisions
if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
{
return;
}
//skip duplicates (disabled for now)
//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
// return;
//search for shared vertices and edges
int numshared = 0;
int[] sharedVertsA = new int[] { -1, -1, -1 };
int[] sharedVertsB = new int[] { -1, -1, -1 };
///skip degenerate triangles
float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared();
if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared();
///skip degenerate triangles
if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
#if false
printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
m_triangleVerticesA[0].GetX(),m_triangleVerticesA[0].GetY(),m_triangleVerticesA[0].GetZ(),
m_triangleVerticesA[1].GetX(),m_triangleVerticesA[1].GetY(),m_triangleVerticesA[1].GetZ(),
m_triangleVerticesA[2].GetX(),m_triangleVerticesA[2].GetY(),m_triangleVerticesA[2].GetZ());
printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
triangle[0].GetX(),triangle[0].GetY(),triangle[0].GetZ(),
triangle[1].GetX(),triangle[1].GetY(),triangle[1].GetZ(),
triangle[2].GetX(),triangle[2].GetY(),triangle[2].GetZ());
#endif
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold)
{
sharedVertsA[numshared] = i;
sharedVertsB[numshared] = j;
numshared++;
///degenerate case
if (numshared >= 3)
{
return;
}
}
}
///degenerate case
if (numshared >= 3)
{
return;
}
}
switch (numshared)
{
case 0:
{
break;
}
case 1:
{
//shared vertex
break;
}
case 2:
{
//shared edge
//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
{
sharedVertsA[0] = 2;
sharedVertsA[1] = 0;
int tmp = sharedVertsB[1];
sharedVertsB[1] = sharedVertsB[0];
sharedVertsB[0] = tmp;
}
int hash = GetHash(m_partIdA, m_triangleIndexA);
TriangleInfo info = null;
if (m_triangleInfoMap.ContainsKey(hash))
{
info = m_triangleInfoMap[hash];
}
else
{
info = new TriangleInfo();
m_triangleInfoMap[hash] = info;
}
int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
int otherIndexA = 3 - sumvertsA;
IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
IndexedVector3 normalA;
IndexedVector3 normalB;
tA.CalcNormal(out normalA);
tB.CalcNormal(out normalB);
edge.Normalize();
IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA));
{
IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
if (IndexedVector3.Dot(edgeCrossA, tmp) < 0)
{
edgeCrossA *= -1;
}
}
IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized();
{
IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
if (IndexedVector3.Dot(edgeCrossB, tmp) < 0)
{
edgeCrossB *= -1;
}
}
float angle2 = 0;
float ang4 = 0.0f;
IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB);
float len2 = calculatedEdge.LengthSquared();
float correctedAngle = 0f;
IndexedVector3 calculatedNormalB = normalA;
bool isConvex = false;
if (len2 < m_triangleInfoMap.m_planarEpsilon)
{
angle2 = 0.0f;
ang4 = 0.0f;
}
else
{
calculatedEdge.Normalize();
IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA);
calculatedNormalA.Normalize();
angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB);
ang4 = MathUtil.SIMD_PI - angle2;
float dotA = IndexedVector3.Dot(normalA, edgeCrossB);
///@todo: check if we need some epsilon, due to floating point imprecision
isConvex = (dotA < 0f);
correctedAngle = isConvex ? ang4 : -ang4;
IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle);
IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2);
calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA;
}
//alternatively use
//IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA);
switch (sumvertsA)
{
case 1:
{
IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
float bla = IndexedVector3.Dot(computedNormalB, normalB);
if (bla < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001f)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif//DEBUG_INTERNAL_EDGE
info.m_edgeV0V1Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX;
}
break;
}
case 2:
{
IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV2V0Angle = -correctedAngle;
if (isConvex)
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX;
break;
}
case 3:
{
IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB;
computedNormalB *= -1;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV1V2Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX;
}
break;
}
}
break;
}
default:
{
// printf("warning: duplicate triangle\n");
break;
}
}
}
public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
{
Generic6DofSpringConstraint constrain = null;
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
if (body1 != null && body2 != null)
{
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
frame2._origin = frame1v;
constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
constrain.CalculateTransforms();
}
return new BulletConstraintXNA(constrain);
}
public float Dot(IndexedQuaternion q)
{
return X * q.X + Y * q.Y + Z * q.Z + W * q.W;
}
public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
{
CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
_orientation.W);
IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
mat._origin = vposition;
collisionObject.SetWorldTransform(mat);
}
public IndexedVector3 QuatRotate(IndexedQuaternion rotation, IndexedVector3 v)
{
IndexedQuaternion q = rotation * v;
q *= rotation.Inverse();
return new IndexedVector3(q.X,q.Y,q.Z);
}
//SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
{
Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
frame2._origin = frame2v;
constraint.SetFrames(ref frame1, ref frame2);
return true;
}
public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
{
RigidBody body = (pBody as BulletBodyXNA).rigidBody;
IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
body.SetCenterOfMassTransform( ref mat);
/* TODO: double check this */
}
public void SetRotation(ref IndexedQuaternion q)
{
_basis.SetRotation(ref q);
}
public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
Vector3 pframe1, Quaternion pframe1rot,
Vector3 pframe2, Quaternion pframe2rot,
bool pdisableCollisionsBetweenLinkedBodies)
{
ConeTwistConstraint constrain = null;
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
if (rb1 != null && rb2 != null)
{
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
frame2._origin = frame1v;
constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
}
return new BulletConstraintXNA(constrain);
}
internal static void AddObjectToWorld2(object pWorld, object pBody, Vector3 _position, Quaternion _orientation)
{
RigidBody body = pBody as RigidBody;
DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
//if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
world.AddRigidBody(body);
IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
_orientation.W);
IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
mat._origin = vposition;
body.SetWorldTransform(mat);
//if (body.GetBroadphaseHandle() != null)
// world.UpdateSingleAabb(body);
}
public override BulletConstraint Create6DofConstraintFixed(BulletWorld pWorld, BulletBody pBody1,
Vector3 pframe1, Quaternion pframe1rot,
bool pUseLinearReferenceFrameB, bool pdisableCollisionsBetweenLinkedBodies)
{
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
Generic6DofConstraint consttr = new Generic6DofConstraint(body1, ref frame1, pUseLinearReferenceFrameB);
consttr.CalculateTransforms();
world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
return new BulletConstraintXNA(consttr);
}
internal static void SetTranslation2(object pBody, Vector3 _position, Quaternion _orientation)
{
RigidBody body = pBody as RigidBody;
IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
_orientation.W);
IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
mat._origin = vposition;
body.SetWorldTransform(mat);
}
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 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out 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 = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(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.GetMargin() + min1.GetMargin() + 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();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); ;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref 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)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out 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;
}
IndexedMatrix unPerturbedTransform;
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)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.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._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#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(ref 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();
}
}
//BulletSimAPI.Create6DofConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
internal static object Create6DofConstraint2(object pWorld, object pBody1, object pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
{
DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
RigidBody body1 = pBody1 as RigidBody;
RigidBody body2 = pBody2 as RigidBody;
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
frame2._origin = frame1v;
Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
puseLinearReferenceFrameA);
consttr.CalculateTransforms();
world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
return consttr;
}
