public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle)
{
//IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0));
IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse();
IndexedQuaternion dorn = IndexedQuaternion.Identity;
GetRotation(ref dmat, out dorn);
///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);
//axis[3] = float(0.);
//check for axis length
float len = axis.LengthSquared();
if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
axis = new IndexedVector3(1,0,0);
}
else
{
axis.Normalize();
}
}
///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;
}
//angular constraint between two different rigidbodies
public JacobianEntry(IndexedVector3 jointAxis,
IndexedBasisMatrix world2A,
IndexedBasisMatrix world2B,
IndexedVector3 inertiaInvA,
IndexedVector3 inertiaInvB) : this(ref jointAxis, ref world2A, ref world2B, ref inertiaInvA, ref inertiaInvB)
{
}
public virtual void InternalProcessTriangleIndex(IndexedVector3[] triangle, int partId, int triangleIndex)
{
IndexedBasisMatrix i = new IndexedBasisMatrix();
IndexedVector3 a = triangle[0] - m_center;
IndexedVector3 b = triangle[1] - m_center;
IndexedVector3 c = triangle[2] - m_center;
float volNeg = -Math.Abs(a.Triple(ref b, ref c)) * (1.0f / 6.0f);
for (int j = 0; j < 3; j++)
{
for (int k = 0; k <= j; k++)
{
i[j, k] = i[k, j] = volNeg * (0.1f * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+ 0.05f * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
}
}
float i00 = -i._el0.X;
float i11 = -i._el1.Y;
float i22 = -i._el2.Z;
i[0, 0] = i11 + i22;
i[1, 1] = i22 + i00;
i[2, 2] = i00 + i11;
m_sum._el0 += i._el0;
m_sum._el1 += i._el1;
m_sum._el2 += i._el2;
}
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);
}
///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);
}
//constraint on one rigidbody
public JacobianEntry(
IndexedBasisMatrix world2A,
IndexedVector3 rel_pos1, IndexedVector3 rel_pos2,
IndexedVector3 jointAxis,
IndexedVector3 inertiaInvA,
float massInvA) : this(ref world2A, ref rel_pos1, ref rel_pos2, ref jointAxis, ref inertiaInvA, massInvA)
{
}
//! Calcs the full invertion of the matrices. Useful for scaling matrices
public void CalcFromFullInvert(ref IndexedMatrix trans0, ref IndexedMatrix trans1)
{
m_R1to0 = trans0._basis.Inverse();
m_T1to0 = m_R1to0 * (-trans0._origin);
m_T1to0 += m_R1to0 * trans1._origin;
m_R1to0 *= trans1._basis;
CalcAbsoluteMatrix();
}
///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
RigidBody body2, ref IndexedVector3 pos2,
float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
IndexedVector3 vel = vel1 - vel2;
IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();
JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal,
body1.GetInvInertiaDiagLocal(), body1.GetInvMass(),
body2.GetInvInertiaDiagLocal(), body2.GetInvMass());
float jacDiagAB = jac.GetDiagonal();
float jacDiagABInv = 1f / jacDiagAB;
float rel_vel = jac.GetRelativeVelocity(
body1.GetLinearVelocity(),
body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
body2.GetLinearVelocity(),
body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
float a = jacDiagABInv;
rel_vel = normal.Dot(ref vel);
//todo: move this into proper structure
float contactDamping = 0.2f;
if (ONLY_USE_LINEAR_MASS)
{
float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
impulse = -contactDamping * rel_vel * massTerm;
}
else
{
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
}
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);
}
//! Calc the transformation relative 1 to 0. Inverts matrics by transposing
public void CalcFromHomogenic(ref IndexedMatrix trans0, ref IndexedMatrix trans1)
{
IndexedMatrix temp_trans = trans0.Inverse();
temp_trans = temp_trans * trans1;
m_T1to0 = temp_trans._origin;
m_R1to0 = temp_trans._basis;
CalcAbsoluteMatrix();
}
public override void SyncPosition()
{
ChCoordsys mcsys = this.mcontactable.GetCsysForCollisionModel();
bt_collision_object.GetWorldTransform()._origin = new IndexedVector3(
(float)mcsys.pos.x, (float)mcsys.pos.y, (float)mcsys.pos.z);
ChMatrix33 <double> rA = new ChMatrix33 <double>(mcsys.rot);
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]);
bt_collision_object.GetWorldTransform()._basis = basisA;
}
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 static void TransformAabb(ref IndexedVector3 halfExtents, float margin, ref IndexedMatrix t, out IndexedVector3 aabbMinOut, out IndexedVector3 aabbMaxOut)
{
IndexedVector3 halfExtentsWithMargin = halfExtents + new IndexedVector3(margin);
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtentsWithMargin),
abs_b._el1.Dot(ref halfExtentsWithMargin),
abs_b._el2.Dot(ref halfExtentsWithMargin));
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
public override void ClientMoveAndDisplay(GameTime gameTime)
{
IndexedVector3 walkDirection = IndexedVector3.Zero;
float walkVelocity = 1.1f * 4.0f; // 4 km/h -> 1.1 m/s
float walkSpeed = walkVelocity * 0.001f;
IndexedMatrix xform = ghostObject.GetWorldTransform();
IndexedVector3 forwardDir = xform._basis[2];
IndexedVector3 upDir = xform._basis[1];
IndexedVector3 strafeDir = xform._basis[0];
forwardDir.Normalize();
upDir.Normalize();
strafeDir.Normalize();
KeyboardState keyboardState = Keyboard.GetState();
if (keyboardState.IsKeyDown(Keys.I))
{
walkDirection += forwardDir;
}
if (keyboardState.IsKeyDown(Keys.K))
{
walkDirection -= forwardDir;
}
if (keyboardState.IsKeyDown(Keys.J))
{
IndexedMatrix orn = ghostObject.GetWorldTransform();
orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), 0.01f);
ghostObject.SetWorldTransform(orn);
}
if (keyboardState.IsKeyDown(Keys.L))
{
IndexedMatrix orn = ghostObject.GetWorldTransform();
orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), -0.01f);
ghostObject.SetWorldTransform(orn);
}
//if (key == Keys.O)
//{
// playerController.Jump();
//}
IndexedVector3 result = walkDirection * walkSpeed;
playerController.SetWalkDirection(ref result);
base.ClientMoveAndDisplay(gameTime);
}
public JacobianEntry(ref IndexedVector3 jointAxis,
ref IndexedBasisMatrix world2A,
ref IndexedBasisMatrix world2B,
ref IndexedVector3 inertiaInvA,
ref IndexedVector3 inertiaInvB)
{
m_linearJointAxis = IndexedVector3.Zero;
m_aJ = world2A * jointAxis;
m_bJ = world2B * -jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = IndexedVector3.Dot(m_0MinvJt, m_aJ) + IndexedVector3.Dot(m_1MinvJt, m_bJ);
Debug.Assert(m_Adiag > 0.0f);
}
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = new IndexedVector3(GetRadius());
halfExtents[m_upAxis] = GetRadius() + GetHalfHeight();
halfExtents += new IndexedVector3(GetMargin());
IndexedBasisMatrix abs_b = trans._basis.Absolute();
IndexedVector3 center = trans._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents),
abs_b._el1.Dot(ref halfExtents),
abs_b._el2.Dot(ref halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
public JacobianEntry(
ref IndexedBasisMatrix world2A,
ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
ref IndexedVector3 jointAxis,
ref IndexedVector3 inertiaInvA,
float massInvA)
{
m_linearJointAxis = jointAxis;
m_aJ = world2A * (rel_pos1.Cross(ref jointAxis));
m_bJ = world2A * (rel_pos2.Cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = IndexedVector3.Zero;
m_Adiag = massInvA + IndexedVector3.Dot(m_0MinvJt, m_aJ);
Debug.Assert(m_Adiag > 0.0f);
}
public void UpdateInertiaTensor()
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("[{0}] RigidBody updateInertiaTensor", (String)m_userObjectPointer));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "invInertiaLocal", m_invInertiaLocal);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform._basis.Scaled(ref m_invInertiaLocal));
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform._basis.Transpose());
}
m_invInertiaTensorWorld = m_worldTransform._basis.Scaled(ref m_invInertiaLocal) * m_worldTransform._basis.Transpose();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_invInertiaTensorWorld);
}
}
//! Apply a transform to an AABB
public void ApplyTransformTransCache(ref BT_BOX_BOX_TRANSFORM_CACHE trans)
{
IndexedVector3 center = (m_max + m_min) * 0.5f;
IndexedVector3 extends = m_max - center;
// Compute new center
center = trans.Transform(ref center);
IndexedBasisMatrix absMatrix = trans.m_R1to0.Absolute();
IndexedVector3 textends = new IndexedVector3(extends.Dot(trans.m_R1to0.GetRow(0).Absolute()),
extends.Dot(trans.m_R1to0.GetRow(1).Absolute()),
extends.Dot(trans.m_R1to0.GetRow(2).Absolute()));
m_min = center - textends;
m_max = center + textends;
}
public override void ClientMoveAndDisplay(GameTime gameTime)
{
IndexedVector3 walkDirection = IndexedVector3.Zero;
float walkVelocity = 1.1f * 4.0f;
float walkSpeed = walkVelocity * gameTime.ElapsedGameTime.Milliseconds / 1000.0f;
IndexedMatrix xform = m_ghostObject.GetWorldTransform();
IndexedVector3 forwardDir = xform._basis[2];
IndexedVector3 upDir = xform._basis[1];
IndexedVector3 strafeDir = xform._basis[0];
forwardDir.Normalize();
upDir.Normalize();
strafeDir.Normalize();
KeyboardState keyboardState = Keyboard.GetState();
if (keyboardState.IsKeyDown(Keys.I))
{
walkDirection += forwardDir;
}
if (keyboardState.IsKeyDown(Keys.K))
{
walkDirection -= forwardDir;
}
if (keyboardState.IsKeyDown(Keys.J))
{
IndexedMatrix orn = m_ghostObject.GetWorldTransform();
orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), 0.01f);
m_ghostObject.SetWorldTransform(orn);
}
if (keyboardState.IsKeyDown(Keys.L))
{
IndexedMatrix orn = m_ghostObject.GetWorldTransform();
orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), -0.01f);
m_ghostObject.SetWorldTransform(orn);
}
IndexedVector3 result = walkDirection * walkSpeed;
m_character.SetWalkDirection(ref result);
base.ClientMoveAndDisplay(gameTime);
}
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 localHalfExtents = 0.5f * (m_localAabbMax - m_localAabbMin);
float margin = GetMargin();
localHalfExtents += new IndexedVector3(margin);
IndexedVector3 localCenter = 0.5f * (m_localAabbMax + m_localAabbMin);
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 static void TransformAabb(ref IndexedVector3 localAabbMin, ref IndexedVector3 localAabbMax, float margin, ref IndexedMatrix trans, out IndexedVector3 aabbMinOut, out IndexedVector3 aabbMaxOut)
{
Debug.Assert(localAabbMin.X <= localAabbMax.X);
Debug.Assert(localAabbMin.Y <= localAabbMax.Y);
Debug.Assert(localAabbMin.Z <= localAabbMax.Z);
IndexedVector3 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
localHalfExtents += new IndexedVector3(margin);
IndexedVector3 localCenter = 0.5f * (localAabbMax + localAabbMin);
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));
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * 0.5f;
IndexedVector3 localOrigin = IndexedVector3.Zero;
localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * 0.5f;
localOrigin *= m_localScaling;
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents),
abs_b._el1.Dot(ref halfExtents),
abs_b._el2.Dot(ref halfExtents));
extent += new IndexedVector3(GetMargin());
aabbMin = center - extent;
aabbMax = center + extent;
}
///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. A mass of 1 is assumed, for other masses just multiply the computed "inertia"
///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
///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. This method also computes the volume of the convex mesh.
public void CalculatePrincipalAxisTransform(ref IndexedMatrix principal, out IndexedVector3 inertia, float volume)
{
CenterCallback centerCallback = new CenterCallback();
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
m_stridingMesh.InternalProcessAllTriangles(centerCallback, ref aabbMin, ref aabbMax);
IndexedVector3 center = centerCallback.GetCenter();
principal._origin = center;
volume = centerCallback.GetVolume();
InertiaCallback inertiaCallback = new InertiaCallback(ref center);
m_stridingMesh.InternalProcessAllTriangles(inertiaCallback, ref aabbMax, ref aabbMax);
IndexedBasisMatrix i = inertiaCallback.GetInertia();
i.Diagonalize(out principal, 0.00001f, 20);
//i.diagonalize(principal.getBasis(), 0.00001f, 20);
inertia = new IndexedVector3(i[0, 0], i[1, 1], i[2, 2]);
inertia /= volume;
}
//public BoxBoxDetector(BoxShape box1, BoxShape box2)
//{
// m_box1 = box1;
// m_box2 = box2;
//}
// Work in progress to copy redo the box detector to remove un-necessary allocations
public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
int skip = 0;
Object contact = null;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA._origin);
IndexedVector3 translationB = new IndexedVector3(transformB._origin);
//IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());
//IndexedVector3 box1Margin = 2f * debugExtents;
//IndexedVector3 box2Margin = 2f * debugExtents;
IndexedBasisMatrix rotateA = transformA._basis.Transpose();
IndexedBasisMatrix rotateB = transformB._basis.Transpose();
for (int j = 0; j < 3; j++)
{
s_temp1[0 + 4 * j] = transformA._basis[j].X;
s_temp2[0 + 4 * j] = transformB._basis[j].X;
s_temp1[1 + 4 * j] = transformA._basis[j].Y;
s_temp2[1 + 4 * j] = transformB._basis[j].Y;
s_temp1[2 + 4 * j] = transformA._basis[j].Z;
s_temp2[2 + 4 * j] = transformB._basis[j].Z;
}
//s_temp1[0] = rotateA._Row0.X;
//s_temp1[1] = rotateA._Row0.Y;
//s_temp1[2] = rotateA._Row0.Z;
//s_temp1[4] = rotateA._Row1.X;
//s_temp1[5] = rotateA._Row1.Y;
//s_temp1[6] = rotateA._Row1.Z;
//s_temp1[8] = rotateA._Row2.X;
//s_temp1[9] = rotateA._Row2.X;
//s_temp1[10] = rotateA._Row2.X;
//s_temp2[0] = rotateB._Row0.X;
//s_temp2[1] = rotateB._Row0.Y;
//s_temp2[2] = rotateB._Row0.Z;
//s_temp2[4] = rotateB._Row1.X;
//s_temp2[5] = rotateB._Row1.Y;
//s_temp2[6] = rotateB._Row1.Z;
//s_temp2[8] = rotateB._Row2.X;
//s_temp2[9] = rotateB._Row2.Y;
//s_temp2[10] = rotateB._Row2.Z;
DBoxBox2(ref translationA,
s_temp1,
ref box1Margin,
ref translationB,
s_temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
public virtual void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = GetNumWheels();
if (numWheel == 0)
{
return;
}
//m_forwardWS.resize(numWheel);
//m_axle.resize(numWheel);
//m_forwardImpulse.resize(numWheel);
//m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < numWheel; i++)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
m_sideImpulse[i] = 0f;
m_forwardImpulse[i] = 0f;
}
if (numWheelsOnGround != 4)
{
int ibreak = 0;
}
{
//foreach(WheelInfo wheelInfo in m_wheelInfo)
for (int i = 0; i < numWheel; ++i)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
IndexedMatrix wheelTrans = GetWheelTransformWS(i);
IndexedBasisMatrix wheelBasis0 = wheelTrans._basis;
m_axle[i] = new IndexedVector3(
wheelBasis0._el0[m_indexRightAxis],
wheelBasis0._el1[m_indexRightAxis],
wheelBasis0._el2[m_indexRightAxis]);
IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
float proj = IndexedVector3.Dot(m_axle[i], surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i].Normalize();
m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]);
m_forwardWS[i].Normalize();
IndexedVector3 tempAxle = m_axle[i];
float tempImpulse = m_sideImpulse[i];
ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS,
groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS,
0f, ref tempAxle, ref tempImpulse, timeStep);
m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2);
}
}
}
float sideFactor = 1f;
float fwdFactor = 0.5f;
bool sliding = false;
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
float rollingFriction = 0f;
if (groundObject != null)
{
if (wheelInfo.m_engineForce != 0.0f)
{
rollingFriction = wheelInfo.m_engineForce * timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0f;
float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
IndexedVector3 tempWheel = m_forwardWS[wheel];
WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse);
m_forwardWS[wheel] = tempWheel;
rollingFriction = CalcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = 0f;
m_wheelInfo[wheel].m_skidInfo = 1f;
if (groundObject != null)
{
m_wheelInfo[wheel].m_skidInfo = 1f;
float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
float x = (m_forwardImpulse[wheel]) * fwdFactor;
float y = (m_sideImpulse[wheel]) * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = (float)(maximp / Math.Sqrt(impulseSquared));
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
if (m_sideImpulse[wheel] != 0f)
{
if (m_wheelInfo[wheel].m_skidInfo < 1f)
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody.GetCenterOfMassPosition();
if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f)
{
int ibreak = 0;
}
if (m_forwardImpulse[wheel] != 0f)
{
m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
}
if (m_sideImpulse[wheel] != 0f)
{
RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody;
IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject.GetCenterOfMassPosition();
IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos);
//apply friction impulse on the ground
IndexedVector3 temp = -sideImp;
groundObject.ApplyImpulse(ref temp, ref rel_pos2);
}
}
}
}
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 bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return(false);
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
IndexedVector3 color = new IndexedVector3(0, 1, 0);
debugDraw.drawLine(minA, minB, color);
color = new IndexedVector3(1, 1, 1);
IndexedVector3 vec = minB - minA;
float prj2 = IndexedVector3.Dot(minNorm, vec);
debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref input, res, debugDraw, false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDraw.drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return(res.m_hasResult);
}
public virtual void GetAabbNonVirtual(ref IndexedMatrix t, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
switch (m_shapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = this as SphereShape;
float radius = sphereShape.GetImplicitShapeDimensions().X;// * convexShape->getLocalScaling().getX();
float margin = radius + sphereShape.GetMarginNonVirtual();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(margin);
aabbMin = center - extent;
aabbMax = center + extent;
}
break;
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
/* fall through */
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape convexShape = this as BoxShape;
float margin = convexShape.GetMarginNonVirtual();
IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions();
halfExtents += new IndexedVector3(margin);
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
break;
}
case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
{
TriangleShape triangleShape = (TriangleShape)this;
float margin = triangleShape.GetMarginNonVirtual();
for (int i = 0; i < 3; i++)
{
IndexedVector3 vec = new IndexedVector3();
vec[i] = 1f;
IndexedVector3 sv = LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis);
IndexedVector3 tmp = t * sv;
aabbMax[i] = tmp[i] + margin;
vec[i] = -1.0f;
tmp = t * (LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis));
aabbMin[i] = tmp[i] - margin;
}
}
break;
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = this as CapsuleShape;
float r = capsuleShape.GetRadius();
IndexedVector3 halfExtents = new IndexedVector3(r);
int m_upAxis = capsuleShape.GetUpAxis();
halfExtents[m_upAxis] = r + capsuleShape.GetHalfHeight();
float nvMargin = capsuleShape.GetMarginNonVirtual();
halfExtents += new IndexedVector3(nvMargin);
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
break;
case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
{
PolyhedralConvexAabbCachingShape convexHullShape = (PolyhedralConvexAabbCachingShape)this;
float margin = convexHullShape.GetMarginNonVirtual();
convexHullShape.GetNonvirtualAabb(ref t, out aabbMin, out aabbMax, margin);
}
break;
default:
GetAabb(ref t, out aabbMin, out aabbMax);
break;
}
// should never reach here
Debug.Assert(false);
}
