public static bool TestInternalObjects( ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin)
{
float dp = delta_c.Dot(ref axis);
IndexedVector3 localAxis0;
InverseTransformPoint3x3(out localAxis0, ref axis,ref trans0);
IndexedVector3 localAxis1;
InverseTransformPoint3x3(out localAxis1, ref axis,ref trans1);
IndexedVector3 p0;
BoxSupport(ref convex0.m_extents, ref localAxis0, out p0);
IndexedVector3 p1;
BoxSupport(ref convex1.m_extents, ref localAxis1, out p1);
float Radius0 = p0.X*localAxis0.X + p0.Y*localAxis0.Y + p0.Z*localAxis0.Z;
float Radius1 = p1.X*localAxis1.X + p1.Y*localAxis1.Y + p1.Z*localAxis1.Z;
float MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
float MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
float MinMaxRadius = MaxRadius + MinRadius;
float d0 = MinMaxRadius + dp;
float d1 = MinMaxRadius - dp;
float depth = d0<d1 ? d0:d1;
if (depth > dmin)
{
return false;
}
return true;
}
public static IndexedMatrix CreateLookAt(IndexedVector3 cameraPosition, IndexedVector3 cameraTarget, IndexedVector3 cameraUpVector)
{
IndexedVector3 vector3_1 = IndexedVector3.Normalize(cameraPosition - cameraTarget);
IndexedVector3 vector3_2 = IndexedVector3.Normalize(IndexedVector3.Cross(cameraUpVector, vector3_1));
IndexedVector3 vector1 = IndexedVector3.Cross(vector3_1, vector3_2);
IndexedMatrix matrix = IndexedMatrix.Identity;
//matrix._basis = new IndexedBasisMatrix(vector3_2.X, vector1.X, vector3_1.X, vector3_2.Y, vector1.Y, vector3_1.Y, vector3_2.Z, vector1.Z, vector3_1.Z).Transpose();
matrix._basis = new IndexedBasisMatrix(ref vector3_2, ref vector1, ref vector3_1);
matrix._origin = new IndexedVector3(-IndexedVector3.Dot(vector3_2, cameraPosition),
-IndexedVector3.Dot(vector1, cameraPosition),
-IndexedVector3.Dot(vector3_1, cameraPosition));
return(matrix);
}
public static int ClipSegmentToLine(ClipVertex[] vOut, ClipVertex[] vIn,
IndexedVector3 normal, float offset)
{
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = normal.Dot(vIn[0].v) - offset;
float distance1 = normal.Dot(vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
if (distance0 > 0.0f)
{
vOut[numOut].id = vIn[0].id;
}
else
{
vOut[numOut].id = vIn[1].id;
}
++numOut;
}
return numOut;
}
public override IndexedVector3 LocalGetSupportingVertexWithoutMargin(ref IndexedVector3 dir)
{
IndexedVector3 dots = new IndexedVector3(
dir.Dot(ref m_vertices1[0]),
dir.Dot(ref m_vertices1[1]),
dir.Dot(ref m_vertices1[2]));
return m_vertices1[MathUtil.MaxAxis(ref dots)];
}
protected IndexedVector3 ComputeReflectionDirection(ref IndexedVector3 direction, ref IndexedVector3 normal)
{
return direction - (2.0f * direction.Dot(ref normal)) * normal;
}
public static void bt_edge_plane(ref IndexedVector3 e1, ref IndexedVector3 e2, ref IndexedVector3 normal, out Vector4 plane)
{
IndexedVector3 planenormal = (e2-e1).Cross(ref normal);
planenormal.Normalize();
plane = new Vector4(planenormal.ToVector3(), e2.Dot(ref planenormal));
}
public void ProjectionInterval(ref IndexedVector3 direction, out float vmin, out float vmax)
{
IndexedVector3 center = (m_max + m_min) * 0.5f;
IndexedVector3 extend = m_max - center;
float _fOrigin = direction.Dot(ref center);
float _fMaximumExtent = extend.Dot(direction.Absolute());
vmin = _fOrigin - _fMaximumExtent;
vmax = _fOrigin + _fMaximumExtent;
}
private static void DLineClosestApproach(ref IndexedVector3 pa, ref IndexedVector3 ua,
ref IndexedVector3 pb, ref IndexedVector3 ub,
out float alpha, out float beta)
{
IndexedVector3 p = pb - pa;
float uaub = ua.Dot(ref ub);
float q1 = ua.Dot(ref p);
float q2 = -(ub.Dot(ref p));
float d = 1 - uaub * uaub;
if (d <= 0.0001f)
{
// @@@ this needs to be made more robust
alpha = 0f;
beta = 0f;
}
else
{
d = 1f / d;
alpha = (q1 + uaub * q2) * d;
beta = (uaub * q1 + q2) * d;
}
}
public float ComputeAngularImpulseDenominator(ref IndexedVector3 axis)
{
IndexedVector3 vec = axis * GetInvInertiaTensorWorld();
return axis.Dot(ref vec);
}
public static float DistancePointPlane(ref IndexedVector4 plane, ref IndexedVector3 point)
{
return point.Dot(new IndexedVector3(plane.X, plane.Y, plane.Z)) - plane.W;
}
public static float ComputeAngularImpulseDenominator(ref IndexedVector3 axis, ref IndexedBasisMatrix invInertiaWorld)
{
//IndexedVector3 vec = MathUtil.TransposeTransformNormal(axis, invInertiaWorld);
IndexedVector3 vec = axis* invInertiaWorld;
return axis.Dot(ref vec);
}
///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;
}
}
