private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref JVector point, ref JVector normal,
out JVector point1, out JVector point2)
{
JVector mn; JVector.Negate(ref normal, out mn);
JVector sA; SupportMapping(body1, shape1, ref mn, out sA);
JVector sB; SupportMapping(body2, shape2, ref normal, out sB);
JVector.Subtract(ref sA, ref point, out sA);
JVector.Subtract(ref sB, ref point, out sB);
double dot1 = JVector.Dot(ref sA, ref normal);
double dot2 = JVector.Dot(ref sB, ref normal);
JVector.Multiply(ref normal, dot1, out sA);
JVector.Multiply(ref normal, dot2, out sB);
JVector.Add(ref point, ref sA, out point1);
JVector.Add(ref point, ref sB, out point2);
}
public bool InSimplex(JVector w)
{
//check in case lastW is already removed
if (w == _lastW)
{
return(true);
}
//w is in the current (reduced) simplex
int numverts = NumVertices;
for (int i = 0; i < numverts; i++)
{
if (_simplexVectorW[i] == w)
{
return(true);
}
}
return(false);
}
/// <summary>
/// Creates a new instance of the Wheel class.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="car">The RigidBody on which to apply the wheel forces.</param>
/// <param name="position">The position of the wheel on the body (in body space).</param>
/// <param name="radius">The wheel radius.</param>
public Wheel(RigidBody car, JVector position, float radius)
{
this.car = car;
this.Position = position;
raycast = new RaycastCallback(RaycastCallback);
// set some default values.
//this.SideFriction = 455f;
// this.ForwardFriction = 451f;
this.SideFriction = 1.5f;
this.ForwardFriction = 1f;
this.Radius = radius;
this.Inertia = 1.0f;
this.WheelTravel = 0.2f;
//MAX SPEED
this.MaximumAngularVelocity = 200f;
this.NumberOfRays = 10;
}
private void AddCardLayer(JVector startPosition, int angledCards)
{
Debug.Assert(angledCards % 2 == 0);
foreach (int i in Enumerable.Range(0, angledCards))
{
AddCard(
startPosition + new JVector(cardSpacing * i, layerHeight / 2f, 0),
(i % 2 == 0) ? angle : oppositeAngle);
}
for (float distance = 1.5f; distance < angledCards - 0.5; distance += 4)
{
AddCard(startPosition + new JVector(cardSpacing * distance, layerHeight, 0), 0);
}
for (float distance = 3.5f; distance < angledCards - 0.5; distance += 4)
{
AddCard(startPosition + new JVector(cardSpacing * distance, layerHeight + (float)cardThickness, 0), 0);
}
}
private void AddShapeToDrawList(Shape shape, JMatrix ori, JVector pos)
{
Primitives3D.GeometricPrimitive primitive = null;
Matrix scaleMatrix = Matrix.Identity;
if (shape is BoxShape)
{
primitive = primitives[(int)Primitives.box];
scaleMatrix = Matrix.CreateScale(Conversion.ToXNAVector((shape as BoxShape).Size));
}
else if (shape is SphereShape)
{
primitive = primitives[(int)Primitives.sphere];
scaleMatrix = Matrix.CreateScale((shape as SphereShape).Radius);
}
else if (shape is CylinderShape)
{
primitive = primitives[(int)Primitives.cylinder];
CylinderShape cs = shape as CylinderShape;
scaleMatrix = Matrix.CreateScale(cs.Radius, cs.Height, cs.Radius);
}
else if (shape is CapsuleShape)
{
primitive = primitives[(int)Primitives.capsule];
CapsuleShape cs = shape as CapsuleShape;
scaleMatrix = Matrix.CreateScale(cs.Radius * 2, cs.Length, cs.Radius * 2);
}
else if (shape is ConeShape)
{
ConeShape cs = shape as ConeShape;
scaleMatrix = Matrix.CreateScale(cs.Radius, cs.Height, cs.Radius);
primitive = primitives[(int)Primitives.cone];
}
if (primitive != null)
{
primitive.AddWorldMatrix(scaleMatrix * Conversion.ToXNAMatrix(ori) *
Matrix.CreateTranslation(Conversion.ToXNAVector(pos)));
}
}
/// <summary>
/// Returns all triangles which intersect the given axis aligned bounding box.
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayDelta"></param>
/// <param name="triangles"></param>
/// <returns></returns>
#region public int GetTrianglesIntersectingtRay(JVector rayOrigin, JVector rayDelta)
public int GetTrianglesIntersectingRay(List <int> triangles, JVector rayOrigin, JVector rayDelta)
{
if (nodes.Length == 0)
{
return(0);
}
int curStackIndex = 0;
int endStackIndex = 1;
UInt16[] nodeStack = nodeStackPool.GetNew();
nodeStack[0] = 0;
int triCount = 0;
while (curStackIndex < endStackIndex)
{
UInt16 nodeIndex = nodeStack[curStackIndex];
curStackIndex++;
if (nodes[nodeIndex].box.SegmentIntersect(ref rayOrigin, ref rayDelta))
{
for (int i = 0; i < nodes[nodeIndex].triIndices.Length; ++i)
{
if (triBoxes[nodes[nodeIndex].triIndices[i]].SegmentIntersect(ref rayOrigin, ref rayDelta))
{
triangles.Add(nodes[nodeIndex].triIndices[i]);
triCount++;
}
}
int numChildren = nodes[nodeIndex].nodeIndices.Length;
for (int i = 0; i < numChildren; ++i)
{
nodeStack[endStackIndex++] = nodes[nodeIndex].nodeIndices[i];
}
}
}
nodeStackPool.GiveBack(nodeStack);
return(triCount);
}
/// <summary>
/// The points in wolrd space gets recalculated by transforming the
/// local coordinates. Also new penetration depth is estimated.
/// </summary>
public void UpdatePosition()
{
if (!body1IsMassPoint)
{
JVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
}
JVector.Add(ref p1, ref body1.position, out p1);
if (!body2IsMassPoint)
{
JVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
}
JVector.Add(ref p2, ref body2.position, out p2);
JVector dist; JVector.Subtract(ref p1, ref p2, out dist);
penetration = JVector.Dot(ref dist, ref normal);
}
// sort cached points so most isolated points come first
private int SortCachedPoints(ref JVector realRelPos1, float pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
float maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
JVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out var a0);
public void DebugDraw(IDebugDrawer drawer)
{
JVector pos1, pos2, pos3;
for (int i = 0; i < hullPoints.Count; i += 3)
{
pos1 = hullPoints[i + 0];
pos2 = hullPoints[i + 1];
pos3 = hullPoints[i + 2];
JVector.Transform(ref pos1, ref orientation, out pos1);
JVector.Add(ref pos1, ref position, out pos1);
JVector.Transform(ref pos2, ref orientation, out pos2);
JVector.Add(ref pos2, ref position, out pos2);
JVector.Transform(ref pos3, ref orientation, out pos3);
JVector.Add(ref pos3, ref position, out pos3);
drawer.DrawTriangle(pos1, pos2, pos3);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
// this Sqrt is bad!
float r = (float)Math.Sqrt(direction.X * direction.X);
if (Math.Abs(direction.Y) > 0.0f)
{
JVector dir; JVector.Normalize(ref direction, out dir);
JVector.Multiply(ref dir, radius, out result);
result.Y += Math.Sign(direction.Y) * 0.5f * length;
}
else if (r > 0.0f)
{
result.X = direction.X / r * radius;
result.Y = 0.0f;
}
else
{
result.X = 0.0f;
result.Y = 0.0f;
}
}
/// <summary>
/// Uses the supportMapping to calculate the bounding box. Should be overidden
/// to make this faster.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public virtual void GetBoundingBox(ref float rotation, out JBBox box)
{
JVector vec = JVector.Zero;
JMatrix orientation = JMatrix.CreateRotationZ(rotation);
vec.Set(orientation.M11, orientation.M21);
SupportMapping(ref vec, out vec);
box.Max.X = orientation.M11 * vec.X + orientation.M21 * vec.Y;
vec.Set(orientation.M12, orientation.M22);
SupportMapping(ref vec, out vec);
box.Max.Y = orientation.M12 * vec.X + orientation.M22 * vec.Y;
vec.Set(-orientation.M11, -orientation.M21);
SupportMapping(ref vec, out vec);
box.Min.X = orientation.M11 * vec.X + orientation.M21 * vec.Y;
vec.Set(-orientation.M12, -orientation.M22);
SupportMapping(ref vec, out vec);
box.Min.Y = orientation.M12 * vec.X + orientation.M22 * vec.Y;
}
public void SweptExpandBoundingBox(float timestep)
{
sweptDirection = linearVelocity * timestep;
if (sweptDirection.X < 0.0f)
{
boundingBox.Min.X += sweptDirection.X;
}
else
{
boundingBox.Max.X += sweptDirection.X;
}
if (sweptDirection.Y < 0.0f)
{
boundingBox.Min.Y += sweptDirection.Y;
}
else
{
boundingBox.Max.Y += sweptDirection.Y;
}
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (DisableRotation)
{
orientation = 0;
}
// particles don't rotate
if (isParticle)
{
this.inertia = 0.0f;
this.invInertia = 0.0f;
this.invOrientation = this.orientation = 0.0f;
this.boundingBox = shape.boundingBox;
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
angularVelocity = 0.0f;
}
else
{
// Given: Orientation, Inertia
// 3d version
//JMatrix.Transpose(ref orientation, out invOrientation);
invOrientation = -orientation;
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
if (!isStatic)
{
// 3d version
//JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
//JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
// we don't need a world inverse inertia
}
}
}
public static bool IntersectPortal(JVector v0, JVector v1, JVector v2)
{
JVector a = JVector.Zero;
JVector b = v0;
JVector c = v1;
JVector d = v2;
float a1 = (a.X - d.X) * (b.Y - d.Y) - (a.Y - d.Y) * (b.X - d.X);
float a2 = (a.X - c.X) * (b.Y - c.Y) - (a.Y - c.Y) * (b.X - c.X);
if (a1 != 0.0f && a2 != 0.0f && a1 * a2 < 0.0f)
{
float a3 = (c.X - a.X) * (d.Y - a.Y) - (c.Y - a.Y) * (d.X - a.X);
float a4 = a3 + a2 - a1;
if (a3 != 0.0f && a4 != 0.0f && a3 * a4 < 0.0f)
{
return(true);
}
}
// segments not intersecting
return(false);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
effectiveMass = body1.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
JMatrix.Inverse(ref effectiveMass, out effectiveMass);
JMatrix q = JMatrix.Transpose(orientation) * body1.orientation;
JVector axis;
float x = q.M32 - q.M23;
float y = q.M13 - q.M31;
float z = q.M21 - q.M12;
float r = JMath.Sqrt(x * x + y * y + z * z);
float t = q.M11 + q.M22 + q.M33;
float angle = (float)Math.Atan2(r, t - 1);
axis = new JVector(x, y, z) * angle;
if (r != 0.0f)
{
axis = axis * (1.0f / r);
}
bias = axis * biasFactor * (-1.0f / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += JVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
}
public override void Update(uint deltaMs)
{
var entities = OwnerApp.EntityManager.WithComponent <MovableComponent>().ToArray();
foreach (var entity in entities)
{
var ridgidBodyComponent = OwnerApp.EntityManager.GetComponent <RigidBodyComponent>(entity);
var movableComponent = OwnerApp.EntityManager.GetComponent <MovableComponent>(entity);
var velocity = JVector.Multiply(CalcVelocity(movableComponent.Speed), deltaMs / 16.6f);
if (velocity != JVector.Zero && !(ridgidBodyComponent.Body is null))
{
ridgidBodyComponent.Body.ApplyImpulse(velocity);
}
if (Keyboard.GetState().IsKeyDown(Key.Space)) // TODO && !ridgidBodyComponent.Body.IsActive)
{
ridgidBodyComponent.Body.ApplyImpulse(new JVector(0.0f, movableComponent.JumpSpeed, 0.0f));
}
}
}
internal static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
{
// get capsule endpoints
var p0 = centerB - axis * (length * 0.5f);
var p1 = centerB + axis * (length * 0.5f);
// get vector from endpoint to circle
var D = centerA - p0;
// project vector onto axis and clamp
var d = JVector.Dot(D, axis);
d = JMath.Clamp(d, 0, length);
// get point on axis
var R = p0 + axis * d;
// distance
var b = Math.Abs((centerA - R).Length());
normal = (centerA - R) / b;
// calculate closest 2 points
var RH = JVector.Normalize(centerA - R);
pointA = JVector.Negate(RH) * radiusA + centerA;
pointB = RH * radiusB + R;
normal.Negate();
distance = b - (radiusA + radiusB);
//
if (b < radiusA + radiusB)
{
return(true);
}
return(false);
}
/// <summary>
/// Adds a contact to the arbiter (threadsafe). No more than four contacts
/// are stored in the contactList. When adding a new contact
/// to the arbiter the existing are checked and the best are kept.
/// </summary>
/// <param name="point1">Point on body1. In world space.</param>
/// <param name="point2">Point on body2. In world space.</param>
/// <param name="normal">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void AddContact(JVector point1, JVector point2, JVector normal, JVector[] triangle,
float penetration, ContactSettings contactSettings)
{
// Returning false means that the contact doesn't need to be kept (likely because data from the contact
// was used to manually update controllers).
if (!World.Events.RaiseContactCreated(body1, body2, point1, point2, normal, triangle, penetration))
{
return;
}
JVector.Subtract(ref point1, ref body1.position, out var relPos1);
lock (contactList)
{
int index;
if (contactList.Count == 4)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, triangle, penetration, index, contactSettings);
// TODO: I added this return (to prevent contact count blowing up). Is this correct?
return;
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, triangle, penetration, index, contactSettings);
}
Contact contact = Contact.Pool.GetNew();
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, triangle, penetration, true,
contactSettings);
contactList.Add(contact);
}
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
bool leftTriangle = false;
if (index >= numX * numZ)
{
leftTriangle = true;
index -= numX * numZ;
}
int quadIndexX = index % numX;
int quadIndexZ = index / numX;
// each quad has two triangles, called 'leftTriangle' and !'leftTriangle'
if (leftTriangle)
{
points[0].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
else
{
points[0].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
JVector sum = points[0];
JVector.Add(ref sum, ref points[1], out sum);
JVector.Add(ref sum, ref points[2], out sum);
JVector.Multiply(ref sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(ref points[1], ref points[0], out sum);
JVector.Subtract(ref points[2], ref points[0], out normal);
JVector.Cross(ref sum, ref normal, out normal);
}
private void UpdateArbiterContacts(Arbiter arbiter)
{
if (arbiter.contactList.Count == 0)
{
lock (removedArbiterStack) { removedArbiterStack.Push(arbiter); }
return;
}
for (int i = arbiter.contactList.Count - 1; i >= 0; i--)
{
Contact c = arbiter.contactList[i];
c.UpdatePosition();
if (c.penetration < -contactSettings.breakThreshold)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
else
{
JVector diff; JVector.Subtract(ref c.p1, ref c.p2, out diff);
double distance = JVector.Dot(ref diff, ref c.normal);
diff = diff - distance * c.normal;
distance = diff.LengthSquared();
// hack (multiplication by factor 100) in the
// following line.
if (distance > contactSettings.breakThreshold * contactSettings.breakThreshold * 100)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
}
}
}
public override void Update(float timestep)
{
if (Body.IsStaticOrInactive)
{
return;
}
var worldBodyAnchor = Body.LocalToWorld(LocalAnchor);
var difference = worldBodyAnchor - WorldAnchor;
var differenceMag = difference.Length();
if (IsOnlyPull)
{
if (differenceMag < Length)
{
return;
}
}
if (JMath.IsNearlyZero(differenceMag))
{
return;
}
var diffNormal = JVector.Normalize(difference);
SpringError = differenceMag - Length;
var springForce = SpringConstant * SpringError;
var force = diffNormal * -springForce;
if (!force.IsNearlyZero())
{
Body.AddForce(force, worldBodyAnchor);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
float sigma = (float)Math.Sqrt((float)(direction.X * direction.X + direction.Z * direction.Z));
if (direction.Y > direction.Length() * sina)
{
result.X = 0.0f;
result.Y = (2.0f / 3.0f) * height;
result.Z = 0.0f;
}
else if (sigma > 0.0f)
{
result.X = radius * direction.X / sigma;
result.Y = -(1.0f / 3.0f) * height;
result.Z = radius * direction.Z / sigma;
}
else
{
result.X = 0.0f;
result.Y = -(1.0f / 3.0f) * height;
result.Z = 0.0f;
}
}
public void DrawTriangle(JVector pos1, JVector pos2, JVector pos3)
{
JVector n = JVector.Cross(pos2 - pos1, pos3 - pos1);
n.Normalize();
Vector3 xn = new Vector3(n.X, n.Y, n.Z);
VertexPositionColorNormal[] tri = new VertexPositionColorNormal[3] {
new VertexPositionColorNormal(new Vector3(pos1.X, pos1.Y, pos1.Z), Color.Green, xn),
new VertexPositionColorNormal(new Vector3(pos3.X, pos3.Y, pos3.Z), Color.Green, xn),
new VertexPositionColorNormal(new Vector3(pos2.X, pos2.Y, pos2.Z), Color.Green, xn),
};
effect.Parameters["World"].SetValue(Matrix.Identity);
effect.Parameters["ViewProj"].SetValue(Camera.CurrentCamera.View * Camera.CurrentCamera.Projection);
effect.CurrentTechnique = effect.Techniques["VBO"];
foreach (EffectPass p in effect.CurrentTechnique.Passes)
{
p.Apply();
device.DrawUserPrimitives(PrimitiveType.TriangleList, tri, 0, 1);
}
}
private void AddPressureForces(float timeStep)
{
if (pressure == 0.0f || volume == 0.0f)
{
return;
}
float invVolume = 1.0f / volume;
foreach (Triangle t in triangles)
{
JVector v1 = points[t.indices.I0].position;
JVector v2 = points[t.indices.I1].position;
JVector v3 = points[t.indices.I2].position;
JVector cross = (v3 - v1) % (v2 - v1);
JVector center = (v1 + v2 + v3) * (1.0f / 3.0f);
points[t.indices.I0].AddForce(invVolume * cross * pressure);
points[t.indices.I1].AddForce(invVolume * cross * pressure);
points[t.indices.I2].AddForce(invVolume * cross * pressure);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
double r = (double)Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z);
if (Math.Abs(direction.Y) > 0.0f)
{
JVector dir; JVector.Normalize(ref direction, out dir);
JVector.Multiply(ref dir, radius, out result);
result.Y += Math.Sign(direction.Y) * 0.5f * length;
}
else if (r > 0.0f)
{
result.X = direction.X / r * radius;
result.Y = 0.0f;
result.Z = direction.Z / r * radius;
}
else
{
result.X = 0.0f;
result.Y = 0.0f;
result.Z = 0.0f;
}
}
public override void SetCurrentShape(int index)
{
bool leftTriangle = false;
if (index >= numX * numZ)
{
leftTriangle = true;
index -= numX * numZ;
}
int quadIndexX = index % numX;
int quadIndexZ = index / numX;
if (leftTriangle)
{
points[0] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[2] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
else
{
points[0] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
points[2] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
var sum = points[0];
JVector.Add(sum, points[1], out sum);
JVector.Add(sum, points[2], out sum);
JVector.Multiply(sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(points[1], points[0], out sum);
JVector.Subtract(points[2], points[0], out normal);
JVector.Cross(sum, normal, out normal);
}
private void CollisionDetected(RigidBody body1, RigidBody body2, JVector point1, JVector point2, JVector normal, float penetration)
{
Arbiter arbiter = null;
lock (arbiterMap)
{
arbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = poolArbiter.GetNew();
arbiter.Prepare(poolArbiter, poolContact);
arbiter.body1 = body1; arbiter.body2 = body2;
arbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
addedArbiterQueue.Enqueue(arbiter);
events.RaiseBodiesBeginCollide(body1, body2);
}
}
Contact contact = null;
if (arbiter.body1 == body1)
{
JVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (contact != null)
{
events.RaiseContactCreated(contact);
}
}
public Contact AddContact(
JVector point1,
JVector point2,
JVector normal,
float penetration,
ContactSettings contactSettings)
{
JVector.Subtract(point1, body1.position, out var relPos1);
int index;
lock (contactList)
{
if (contactList.Count == 4)
{
index = SortCachedPoints(relPos1, penetration);
ReplaceContact(point1, point2, normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(point1, point2, normal, penetration, index, contactSettings);
return(null);
}
else
{
var contact = Contact.Pool.GetNew();
contact.Initialize(body1, body2, point1, point2, normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
/// <summary>
/// Detect a possible collision between the object and a planned end-position.
/// </summary>
/// <param name="rigidBody">Rigid-body to test for</param>
/// <param name="start">Start of the test-ray</param>
/// <param name="end">End of the test-ray</param>
/// <returns>Position on the ray which is still possible to place the rigid-body without collision</returns>
public JVector DetectCollision(RigidBody rigidBody, JVector start, JVector end)
{
JVector p = rigidBody.Position;
JVector d = end - start;
JVector d5 = 5 * d;
Collider collider = rigidBody as Collider;
RigidBody resBody;
JVector hitNormal;
float fraction;
bool result = World.CollisionSystem.Raycast(p, d5, RaycastCallback,
out resBody, out hitNormal, out fraction);
if (result && resBody != rigidBody)
{
float maxLength = d.LengthSquared();
JVector collisionAt = p + fraction * d5;
JVector position = GetPosition(collider, collisionAt, hitNormal);
float ncLength = (position - p).LengthSquared();
//resBody.Tag = BodyTag.DrawMe;
//Debug.Log(fraction);
//Debug.Log((resBody as Collider).GameObject.name);
//World.AddBody(new RigidBody(new CylinderShape(1.0f, .25f)) { Position = p, IsStatic = true, PureCollider = true });
//World.AddBody(new RigidBody(new CylinderShape(2.5f, 0.5f)) { Position = end, IsStatic = true, PureCollider = true });
////World.AddBody(new RigidBody(new CylinderShape(1.0f, 1.0f)) { Position = p + d5, IsStatic = true, PureCollider = true });
//World.AddBody(new RigidBody(new CylinderShape(1.0f, 1.0f)) { Position = collisionAt, IsStatic = true, PureCollider = true });
return(maxLength < ncLength ? end : position);
}
return(end);
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
vecs[0] = octree.GetVertex(octree.tris[potentialTriangles[index]].I0);
vecs[1] = octree.GetVertex(octree.tris[potentialTriangles[index]].I1);
vecs[2] = octree.GetVertex(octree.tris[potentialTriangles[index]].I2);
JVector sum = vecs[0];
JVector.Add(ref sum, ref vecs[1], out sum);
JVector.Add(ref sum, ref vecs[2], out sum);
JVector.Multiply(ref sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(ref vecs[1], ref vecs[0], out sum);
JVector.Subtract(ref vecs[2], ref vecs[0], out normal);
JVector.Cross(ref sum, ref normal, out normal);
if (flipNormal)
{
normal.Negate();
}
}
protected virtual void Update()
{
position = cacheTransform.position.ConvertToJVector();
orientation = cacheTransform.rotation.ConvertToJMatrix();
}
//object locker = new object();
private void CollisionDetected(RigidBody body1, RigidBody body2, JVector point1, JVector point2, JVector normal,
float penetration)
{
Arbiter arbiter;
arbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1;
arbiter.body2 = body2;
arbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
addedArbiterQueue.Enqueue(arbiter);
events.RaiseBodiesBeginCollide(body1, body2);
}
Contact contact;
if (arbiter.body1 == body1)
{
JVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (contact != null) events.RaiseContactCreated(contact);
}
