/// <summary>
/// Removes element to the rigid body, you have to specify if you want to compute the inertia tensor, yes in case you will not remove another one or change mass
/// </summary>
public void RemoveElement(MyRBElement element, bool recomputeInertia)
{
MyPhysicsObjects physobj = MyPhysics.physicsSystem.GetPhysicsObjects();
for (int i = 0; i < m_RBElementList.Count; i++)
{
if (m_RBElementList[i] == element)
{
element.SetRigidBody(null);
physobj.RemoveRBElement(element);
m_RBElementList.RemoveAt(i);
break;
}
}
if ((m_Flags & RigidBodyFlag.RBF_INSERTED) > 0)
{
MyPhysics.physicsSystem.GetRigidBodyModule().GetBroadphase().DestroyVolume(element);
}
if (recomputeInertia && m_RBElementList.Count > 0)
{
MyPhysicsUtils.ComputeIntertiaTensor(this);
}
}
/// <summary>
/// Updates matrix from velocities used by old solver
/// </summary>=
public void UpdateMatrix(float dt)
{
m_LinearAcceleration = Vector3.Zero;
m_AngularAcceleration = Vector3.Zero;
m_ExternalAngularAcceleration = Vector3.Zero;
m_ExternalLinearAcceleration = Vector3.Zero;
Vector3 translation = m_Matrix.Translation + m_Velocity * dt;
m_Matrix.Translation = Vector3.Zero;
Vector3 dir = AngularVelocity;
float ang = dir.Length();
if (ang > 0.0f)
{
Vector3.Divide(ref dir, ang, out dir); // dir /= ang;
ang *= dt;
Matrix rot;
Matrix.CreateFromAxisAngle(ref dir, ang, out rot);
Matrix.Multiply(ref m_Matrix, ref rot, out m_Matrix);
}
MyPhysicsUtils.Orthonormalise(ref m_Matrix);
m_Matrix.Translation = translation;
m_LinearAcceleration = Vector3.Zero;
m_AngularAcceleration = Vector3.Zero;
}
/// <summary>
/// Gets the minimum and maximum extents of the triangleVertexes along the axis
/// </summary>
/// <param name="min"></param>
/// <param name="max"></param>
/// <param name="axis"></param>
public void GetSpan(out float min, out float max, Vector3 axis)
{
float d0 = Vector3.Dot(GetPoint(0), axis);
float d1 = Vector3.Dot(GetPoint(1), axis);
float d2 = Vector3.Dot(GetPoint(2), axis);
min = MyPhysicsUtils.Min(d0, d1, d2);
max = MyPhysicsUtils.Max(d0, d1, d2);
}
public void SetMass(float mass, bool recomputeInertiaTensor)
{
System.Diagnostics.Debug.Assert(mass > 0);
m_Mass = mass;
m_OneOverMass = 1.0f / m_Mass;
if (recomputeInertiaTensor)
{
MyPhysicsUtils.ComputeIntertiaTensor(this);
}
}
/// <summary>
/// Adds element to the rigid body, you have to specify if you want to compute the inertia tensor, yes in case you will not insert another one or change mass
/// </summary>
public bool AddElement(MyRBElement element, bool recomputeInertia)
{
m_RBElementList.Add(element);
element.SetRigidBody(this);
if ((m_Flags & RigidBodyFlag.RBF_INSERTED) > 0)
{
MyPhysics.physicsSystem.GetRigidBodyModule().GetBroadphase().CreateVolume(element);
}
if (recomputeInertia)
{
MyPhysicsUtils.ComputeIntertiaTensor(this);
}
return(true);
}
// BEN-OPTIMISATION: New method, ref axis
/// <summary>
/// Gets the minimum and maximum extents of the triangle along the axis
/// </summary>
/// <param name="min"></param>
/// <param name="max"></param>
/// <param name="axis"></param>
public void GetSpan(out float min, out float max, ref Vector3 axis)
{
Vector3 point = new Vector3();
GetPoint(ref point, 0);
float d0 = point.X * axis.X + point.Y * axis.Y + point.Z * axis.Z;
GetPoint(ref point, 1);
float d1 = point.X * axis.X + point.Y * axis.Y + point.Z * axis.Z;
GetPoint(ref point, 2);
float d2 = point.X * axis.X + point.Y * axis.Y + point.Z * axis.Z;
min = MyPhysicsUtils.Min(d0, d1, d2);
max = MyPhysicsUtils.Max(d0, d1, d2);
}
public override void UpdateAABB()
{
Vector3 origin = GetGlobalTransformation().Translation;
m_AABB.Max = origin;
m_AABB.Min = origin;
Vector3 extent = new Vector3();
extent.X = m_Height + m_Radius;
extent.Y = m_Height + m_Radius;
extent.Z = m_Height + m_Radius;
MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, extent);
base.UpdateAABB();
}
public MyPhysics()
{
physicsSystem = this;
m_RigidBodyModule = new MyRigidBodyModule();
m_SensorModule = new MySensorModule();
m_ContactConstraintModule = new MyContactConstraintModule();
m_SensorInteractionModule = new MySensorInteractionModule();
m_RBInteractionModule = new MyRBInteractionModule();
m_Utils = new MyPhysicsUtils();
m_PhysicsObjects = new MyPhysicsObjects();
m_SimulationHandlers = new List <MyPhysSimulationHandler>(16);
}
public override void UpdateAABB()
{
Matrix matrix = GetGlobalTransformation();
m_AABB = m_AABB.CreateInvalid();
BoundingBox box = new BoundingBox(-m_extent, m_extent);
box.GetCorners(m_points);
Vector3 point2;
Vector3 point1;
foreach (Vector3 point in m_points)
{
point1 = point;
Vector3.Transform(ref point1, ref matrix, out point2);
MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, point2);
}
base.UpdateAABB();
}
/// <summary>
/// Solves the solver rbos and constraints through iterations given by the island. This is a newton based solver.
/// </summary>
private void Solve(float dt)
{
Vector3 v1 = new Vector3();
Vector3 v2 = new Vector3();
for (int iteration = 0; iteration < m_Island.IterationCount; iteration++)
{
for (int i = 0; i < m_SolverConstraints.Count; i++)
{
MyRBSolverConstraint sc = m_SolverConstraints[i];
MyRBSolverBody b1 = sc.m_SolverBody1;
MyRBSolverBody b2 = sc.m_SolverBody2;
// compute relative constraint velocity
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Static)
{
v1 = Vector3.Zero;
}
else
{
v1 = b1.m_LinearVelocity + Vector3.Cross(b1.m_AngularVelocity, sc.m_Body1LocalPoint);
}
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Static)
{
v2 = Vector3.Zero;
}
else
{
v2 = b2.m_LinearVelocity + Vector3.Cross(b2.m_AngularVelocity, sc.m_Body2LocalPoint);
}
float current = Vector3.Dot(v2, sc.m_Normal) - Vector3.Dot(v1, sc.m_Normal);
float diff = sc.m_Target - current;
if (sc.m_Magnitude < sc.m_MinMagnitude || sc.m_Magnitude > sc.m_MaxMagnitude ||
(diff < -m_CollisionEpsilon && sc.m_Magnitude > sc.m_MinMagnitude) ||
(diff > m_CollisionEpsilon && sc.m_Magnitude < sc.m_MaxMagnitude))
{
// Alter magnitude
float alter = (diff / sc.m_Affection) * 0.5f;
sc.m_Magnitude += alter;
//Ensure magnitude is in valid range
if (sc.m_Magnitude > sc.m_MaxMagnitude)
{
alter -= sc.m_Magnitude - sc.m_MaxMagnitude;
sc.m_Magnitude = sc.m_MaxMagnitude;
}
else if (sc.m_Magnitude < sc.m_MinMagnitude)
{
alter -= sc.m_Magnitude - sc.m_MinMagnitude;
sc.m_Magnitude = sc.m_MinMagnitude;
}
//Apply impulse
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
b1.m_LinearVelocity -= sc.m_Normal * (b1.m_OneOverMass * alter);
Vector3 tempV = b1.m_LinearVelocity;
tempV.Normalize();
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
float massRatio = 1.0f / (b1.m_OneOverMass / b2.m_OneOverMass);
//If mass of object1 is more than object2, do not apply restitution at all
float rest = MathHelper.Lerp(sc.m_Restitution, 1, massRatio - 0.5f);
sc.m_Restitution = MathHelper.Clamp(rest, sc.m_Restitution, 1);
}
b1.m_AngularVelocity -= sc.m_Body1LocalPointCrossedNormal * (alter);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(b1.m_AngularVelocity);
}
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
b2.m_LinearVelocity += sc.m_Normal * (b2.m_OneOverMass * alter);
Vector3 tempV = b2.m_LinearVelocity;
tempV.Normalize();
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
float massRatio = 1.0f / (b2.m_OneOverMass / b1.m_OneOverMass);
//If mass of object2 is more than object1, do not apply restitution at all
float rest = MathHelper.Lerp(sc.m_Restitution, 1, massRatio - 0.5f);
sc.m_Restitution = MathHelper.Clamp(rest, sc.m_Restitution, 1);
}
b2.m_AngularVelocity += sc.m_Body2LocalPointCrossedNormal * (alter);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(b2.m_AngularVelocity);
}
}
}
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
float cdt = dt / (float)(iteration + 1);
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
//Integrate velocities
body.m_LinearVelocity += body.m_LinearAcceleration * (cdt);
body.m_AngularVelocity += body.m_AngularAcceleration * (cdt);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body.m_AngularVelocity);
if (body.m_MaxAngularVelocity > 0.0f && body.m_AngularVelocity.Length() > body.m_MaxAngularVelocity)
{
body.m_AngularVelocity = MyMwcUtils.Normalize(body.m_AngularVelocity);
body.m_AngularVelocity *= body.m_MaxAngularVelocity;
}
if (body.m_MaxLinearVelocity > 0.0f && body.m_LinearVelocity.Length() > body.m_MaxLinearVelocity)
{
body.m_LinearVelocity = MyMwcUtils.Normalize(body.m_LinearVelocity);
body.m_LinearVelocity *= body.m_MaxLinearVelocity;
}
}
}
}
//Integrate position and orientation
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
if (body.m_State != MyRBSolverBody.SolverBodyState.SBS_Static)
{
Vector3 transl = body.m_Matrix.Translation + body.m_LinearVelocity * (dt);
Vector3 dir = body.m_AngularVelocity;
float ang = dir.Length();
if (ang > 0.0f)
{
body.m_Matrix.Translation = Vector3.Zero;
Vector3.Divide(ref dir, ang, out dir); // dir /= ang;
ang *= dt;
Matrix rot;
Matrix.CreateFromAxisAngle(ref dir, ang, out rot);
Matrix.Multiply(ref body.m_Matrix, ref rot, out body.m_Matrix);
MyPhysicsUtils.Orthonormalise(ref body.m_Matrix);
}
body.m_Matrix.Translation = transl;
}
}
}
protected override bool Interact(bool staticCollision)
{
if (!staticCollision)
{
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("BoxBoxInteraction");
}
try
{
MyRBBoxElement rbbox0 = (MyRBBoxElement)RBElement1;
MyRBBoxElement rbbox1 = (MyRBBoxElement)RBElement2;
MyBox box0 = m_TempBox1;
MyBox box1 = m_TempBox2;
Matrix matrix0 = rbbox0.GetGlobalTransformation();
Matrix matrix1 = rbbox1.GetGlobalTransformation();
box0.Transform.Orientation = matrix0;
box0.Transform.Orientation.Translation = Vector3.Zero;
box0.Transform.Position = matrix0.Translation - Vector3.TransformNormal(rbbox0.Size * 0.5f, matrix0);
box1.Transform.Orientation = matrix1;
box1.Transform.Orientation.Translation = Vector3.Zero;
box1.Transform.Position = matrix1.Translation - Vector3.TransformNormal(rbbox1.Size * 0.5f, matrix1);
box0.SideLengths = rbbox0.Size;
box1.SideLengths = rbbox1.Size;
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
for (int i = 0; i < 15; ++i)
{
switch (i)
{
case 0: seperatingAxes[0] = box0.Orientation.Right; break;
case 1: seperatingAxes[1] = box0.Orientation.Up; break;
case 2: seperatingAxes[2] = box0.Orientation.Backward; break;
case 3: seperatingAxes[3] = box1.Orientation.Right; break;
case 4: seperatingAxes[4] = box1.Orientation.Up; break;
case 5: seperatingAxes[5] = box1.Orientation.Backward; break;
case 6: Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]); break;
case 7: Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]); break;
case 8: Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]); break;
case 9: Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]); break;
case 10: Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]); break;
case 11: Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]); break;
case 12: Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]); break;
case 13: Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]); break;
case 14: Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]); break;
}
// If we can't normalise the axis, skip it
if (seperatingAxes[i].LengthSquared() < MyPhysicsConfig.CollisionEpsilon)
{
continue;
}
overlapDepth[i] = float.MaxValue;
if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, MyPhysicsConfig.CollisionEpsilon))
{
return(false);
}
}
if (staticCollision)
{
return(true); // Static collision: we're done.
}
// Dynamic collision.
// The boxes overlap, find the seperation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (int i = 0; i < 15; ++i)
{
// If we can't normalise the axis, skip it
float l2 = seperatingAxes[i].LengthSquared();
if (l2 < MyPhysicsConfig.CollisionEpsilon)
{
continue;
}
// Normalise the separation axis and depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
seperatingAxes[i] *= invl;
overlapDepth[i] *= invl;
// If this axis is the minmum, select it
if (overlapDepth[i] < minDepth)
{
minDepth = overlapDepth[i];
minAxis = i;
}
}
if (minAxis == -1)
{
return(false);
}
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = box1.GetCentre() - box0.GetCentre();
Vector3 N = seperatingAxes[minAxis];
float depth = overlapDepth[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
{
N *= -1.0f;
}
float minA = MathHelper.Min(box0.SideLengths.X, MathHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z));
float minB = MathHelper.Min(box1.SideLengths.X, MathHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z));
float combinationDist = 0.05f * MathHelper.Min(minA, minB);
// the contact points
contactPts.Clear();
int numPts = contactPts.Count;
GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, MyPhysicsConfig.CollisionEpsilon);
numPts = contactPts.Count;
MyRigidBody rbo0 = GetRigidBody1();
MyRigidBody rbo1 = GetRigidBody2();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
Vector3 body0OldPos = rbo0.Position;
Vector3 body1OldPos = rbo1.Position;
Vector3 body0NewPos = (rbo0.Position + rbo0.LinearVelocity * dt);
Vector3 body1NewPos = (rbo1.Position + rbo1.LinearVelocity * dt);
#region REFERENCE: Vector3 bodyDelta = body0NewPos - body0OldPos - body1NewPos + body1OldPos;
Vector3 bodyDelta;
Vector3.Subtract(ref body0NewPos, ref body0OldPos, out bodyDelta);
Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta);
Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta);
#endregion
#region REFERENCE: float bodyDeltaLen = Vector3.Dot(bodyDelta,N);
float bodyDeltaLen;
Vector3.Dot(ref bodyDelta, ref N, out bodyDeltaLen);
#endregion
float oldDepth = depth + bodyDeltaLen;
MySmallCollPointInfo[] collPtArray = MyContactInfoCache.SCPIStackAlloc();
{
int numCollPts = 0;
Vector3 SATPoint;
switch (minAxis)
{
// Box0 face, Box1 corner collision
case 0:
case 1:
case 2:
{
// Get the lowest point on the box1 along box1 normal
GetSupportPoint(out SATPoint, box1, -N);
break;
}
// We have a Box2 corner/Box1 face collision
case 3:
case 4:
case 5:
{
// Find with vertex on the triangleVertexes collided
GetSupportPoint(out SATPoint, box0, N);
break;
}
// We have an edge/edge collision
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
{
{
// Retrieve which edges collided.
int i = minAxis - 6;
int ia = i / 3;
int ib = i - ia * 3;
// find two P0, P1 point on both edges.
Vector3 P0, P1;
GetSupportPoint(out P0, box0, N);
GetSupportPoint(out P1, box1, -N);
// Find the edge intersection.
// plane along N and F, and passing through PB
Vector3 box0Orient, box1Orient;
MyPhysicsUtils.MyPhysicsUnsafe.Get(ref box0.Transform.Orientation, ia, out box0Orient);
MyPhysicsUtils.MyPhysicsUnsafe.Get(ref box1.Transform.Orientation, ib, out box1Orient);
#region REFERENCE: Vector3 planeNormal = Vector3.Cross(N, box1Orient[ib]);
Vector3 planeNormal;
Vector3.Cross(ref N, ref box1Orient, out planeNormal);
#endregion
#region REFERENCE: float planeD = Vector3.Dot(planeNormal, P1);
float planeD;
Vector3.Dot(ref planeNormal, ref P1, out planeD);
#endregion
// find the intersection t, where Pintersection = P0 + t*box edge dir
#region REFERENCE: float div = Vector3.Dot(box0Orient, planeNormal);
float div;
Vector3.Dot(ref box0Orient, ref planeNormal, out div);
#endregion
// plane and ray colinear, skip the intersection.
if (System.Math.Abs(div) < MyPhysicsConfig.CollisionEpsilon)
{
return(false);
}
float t = (planeD - Vector3.Dot(P0, planeNormal)) / div;
// point on edge of box0
#region REFERENCE: P0 += box0Orient * t;
P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0);
#endregion
#region REFERENCE: SATPoint = (P0 + (0.5f * depth) * N);
Vector3.Multiply(ref N, 0.5f * depth, out SATPoint);
Vector3.Add(ref SATPoint, ref P0, out SATPoint);
#endregion
}
break;
}
default:
{
SATPoint = Vector3.Zero;
Debug.Assert(false);
break;
}
}
// distribute the depth according to the distance to the SAT point
if (numPts > 0)
{
float minDist = float.MaxValue;
float maxDist = float.MinValue;
for (int i = 0; i < numPts; ++i)
{
float dist = MyPhysicsUtils.PointPointDistance(contactPts[i].Pos, SATPoint);
if (dist < minDist)
{
minDist = dist;
}
if (dist > maxDist)
{
maxDist = dist;
}
}
// got some intersection points
for (int i = 0; i < numPts; ++i)
{
float minDepthScale = 0.0f;
float dist = MyPhysicsUtils.PointPointDistance(contactPts[i].Pos, SATPoint);
float safeDivisionDist = (maxDist - minDist);
if ((maxDist - minDist) == 0.0f)
{
safeDivisionDist = MyPhysicsConfig.CollisionEpsilon;
}
float depthScale = (dist - minDist) / safeDivisionDist;
depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth;
if (numCollPts < MyPhysicsConfig.MaxContactPoints)
{
collPtArray[numCollPts++] = new MySmallCollPointInfo(contactPts[i].Pos - body0OldPos, contactPts[i].Pos - body1OldPos, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, N, depth, contactPts[i].Pos);
}
}
}
else
{
#region REFERENCE: collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
//collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
Vector3 cp0;
Vector3.Subtract(ref SATPoint, ref body0NewPos, out cp0);
Vector3 cp1;
Vector3.Subtract(ref SATPoint, ref body1NewPos, out cp1);
if (numCollPts < MyPhysicsConfig.MaxContactPoints)
{
collPtArray[numCollPts++] = new MySmallCollPointInfo(cp0, cp1, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, N, oldDepth, SATPoint);
}
#endregion
}
// report Collisions
MyPhysics.physicsSystem.GetContactConstraintModule().AddContactConstraint(this, collPtArray, numCollPts);
}
MyContactInfoCache.FreeStackAlloc(collPtArray);
}
catch
{
throw;
}
finally
{
if (!staticCollision)
{
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
}
}
return(false);
}
/// <summary>
/// The AABox has a corner at the origin and size sides.
/// </summary>
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts,
ref Vector3 sides, MyBox box, ref Vector3 edgePt0, ref Vector3 edgePt1,
ref Matrix origBoxOrient, ref Vector3 origBoxPos,
float combinationDistanceSq)
{
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests. We know that the
// AABox has a corner at the origin
#region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0);
Vector3 edgeDir;
Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir);
MyPhysicsUtils.NormalizeSafe(ref edgeDir);
#endregion
int num = 0;
for (int idir = 3; idir-- != 0;)
{
// skip edge/face tests if nearly parallel
if (System.Math.Abs(MyPhysicsUtils.MyPhysicsUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
int jdir = (idir + 1) % 3;
int kdir = (idir + 2) % 3;
for (int iface = 2; iface-- != 0;)
{
float offset = 0.0f;
if (iface == 1)
{
offset = MyPhysicsUtils.MyPhysicsUnsafe.Get(ref sides, idir);
}
float dist0 = MyPhysicsUtils.MyPhysicsUnsafe.Get(ref edgePt0, idir) - offset;
float dist1 = MyPhysicsUtils.MyPhysicsUnsafe.Get(ref edgePt1, idir) - offset;
float frac = -1.0f;
if (dist0 * dist1 < -MyPhysicsConfig.CollisionEpsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < MyPhysicsConfig.CollisionEpsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < MyPhysicsConfig.CollisionEpsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
#region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1
Vector3 tmp; Vector3 pt;
Vector3.Multiply(ref edgePt1, frac, out tmp);
Vector3.Multiply(ref edgePt0, 1.0f - frac, out pt);
Vector3.Add(ref pt, ref tmp, out pt);
#endregion
// check the point is within the face rectangle
float ptJdir = MyPhysicsUtils.MyPhysicsUnsafe.Get(ref pt, jdir);
float ptKdir = MyPhysicsUtils.MyPhysicsUnsafe.Get(ref pt, kdir);
if ((ptJdir > -MyPhysicsConfig.CollisionEpsilon) &&
(ptJdir < MyPhysicsUtils.MyPhysicsUnsafe.Get(ref sides, jdir) + MyPhysicsConfig.CollisionEpsilon) &&
(ptKdir > -MyPhysicsConfig.CollisionEpsilon) &&
(ptKdir < MyPhysicsUtils.MyPhysicsUnsafe.Get(ref sides, kdir) + MyPhysicsConfig.CollisionEpsilon))
{
// woohoo got a point
#region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient);
Vector3 pos;
Vector3.Transform(ref pt, ref origBoxOrient, out pos);
Vector3.Add(ref origBoxPos, ref pos, out pos);
#endregion
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
public override void UpdateAABB()
{
Matrix matrix = GetGlobalTransformation();
m_AABB = m_AABB.CreateInvalid();
BoundingBox box = new BoundingBox(-m_Size, m_Size);
box.GetCorners(m_points);
Vector3 point2;
Vector3 point1;
foreach (Vector3 point in m_points)
{
point1 = point;
Vector3.Transform(ref point1, ref matrix, out point2);
MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, point2);
}
/*
* Vector3 origin = matrix.Translation;
* m_AABB.Min = origin;
* m_AABB.Max = origin;
*
* Vector3 rotSize = Vector3.TransformNormal(m_Size, GetGlobalTransformation());
*
* m_Extent.X = rotSize.X;
* m_Extent.Y = rotSize.Y;
* m_Extent.Z = rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, origin + m_Extent);
*
* m_Extent.X = -rotSize.X;
* m_Extent.Y = rotSize.Y;
* m_Extent.Z = rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, origin+m_Extent);
*
* m_Extent.X = -rotSize.X;
* m_Extent.Y = -rotSize.Y;
* m_Extent.Z = rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, origin+m_Extent);
*
* m_Extent.X = -rotSize.X;
* m_Extent.Y = -rotSize.Y;
* m_Extent.Z = -rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB, origin+m_Extent);
*
* m_Extent.X = rotSize.X;
* m_Extent.Y = -rotSize.Y;
* m_Extent.Z = rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB,origin+ m_Extent);
*
* m_Extent.X = rotSize.X;
* m_Extent.Y = -rotSize.Y;
* m_Extent.Z = -rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB,origin+ m_Extent);
*
* m_Extent.X = rotSize.X;
* m_Extent.Y = rotSize.Y;
* m_Extent.Z = -rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB,origin+ m_Extent);
*
* m_Extent.X = -rotSize.X;
* m_Extent.Y = rotSize.Y;
* m_Extent.Z = -rotSize.Z;
*
* MyPhysicsUtils.BoundingBoxAddPoint(ref m_AABB,origin+ m_Extent);
*/
base.UpdateAABB();
}
