/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(FPVector force, FPVector pos)
{
FPVector.Add(ref this.force, ref force, out this.force);
FPVector.Subtract(ref pos, ref this.position, out pos);
FPVector.Cross(ref pos, ref force, out pos);
FPVector.Add(ref pos, ref this.torque, out this.torque);
}
/// <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)
{
FPVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
FPVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
FPVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsMassPoint)
{
FPVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
FPVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
FPVector.Add(ref p2, ref body2.position, out p2);
}
FPVector dist; FPVector.Subtract(ref p1, ref p2, out dist);
penetration = FPVector.Dot(ref dist, ref normal);
}
/// <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 Contact AddContact(FPVector point1, FPVector point2, FPVector normal, FP penetration,
ContactSettings contactSettings)
{
FPVector relPos1;
FPVector.Subtract(ref point1, ref body1.position, out relPos1);
int index;
lock (contactList)
{
if (this.contactList.Count == 4)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
else
{
Contact contact = Contact.Pool.GetNew();
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
/// <summary>
/// Raycasts a single body. NOTE: For performance reasons terrain and trianglemeshshape aren't checked
/// against rays (rays are of infinite length). They are checked against segments
/// which start at rayOrigin and end in rayOrigin + rayDirection.
/// </summary>
#region public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out FP fraction)
public override bool Raycast(RigidBody body, FPVector rayOrigin, FPVector rayDirection, out FPVector normal, out FP fraction)
{
fraction = FP.MaxValue; normal = FPVector.zero;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape)
{
Multishape ms = (body.Shape as Multishape).RequestWorkingClone();
FPVector tempNormal; FP tempFraction;
bool multiShapeCollides = false;
FPVector transformedOrigin; FPVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin);
FPVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
FPVector transformedDirection; FPVector.Transform(ref rayDirection, ref body.invOrientation, out transformedDirection);
int msLength = ms.Prepare(ref transformedOrigin, ref transformedDirection);
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (GJKCollide.Raycast(ms, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out tempFraction, out tempNormal))
{
if (tempFraction < fraction)
{
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out tempNormal);
FPVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out tempNormal);
FPVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
normal = tempNormal;
fraction = tempFraction;
multiShapeCollides = true;
}
}
}
ms.ReturnWorkingClone();
return(multiShapeCollides);
}
else
{
return(GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out fraction, out normal));
}
}
public void GetNormal(out FPVector normal)
{
FPVector sum;
FPVector.Subtract(ref owner.points[indices.I1].position, ref owner.points[indices.I0].position, out sum);
FPVector.Subtract(ref owner.points[indices.I2].position, ref owner.points[indices.I0].position, out normal);
FPVector.Cross(ref sum, ref normal, out normal);
}
/// <summary>
/// Initializes a new instance of the DistanceConstraint class.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="anchor1">The anchor point of the first body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
/// <param name="anchor2">The anchor point of the second body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
public PointOnPoint(RigidBody body1, RigidBody body2, FPVector anchor)
: base(body1, body2)
{
FPVector.Subtract(ref anchor, ref body1.position, out localAnchor1);
FPVector.Subtract(ref anchor, ref body2.position, out localAnchor2);
FPVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
FPVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref FPVector point1, ref FPVector point2, ref FPVector n,
FP penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
FPVector.Subtract(ref p1, ref body1.position, out relativePos1);
FPVector.Subtract(ref p2, ref body2.position, out relativePos2);
FPVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
FPVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
accumulatedNormalImpulse = FP.Zero;
accumulatedTangentImpulse = FP.Zero;
lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = FPMath.Max(body1.staticFriction, body2.staticFriction);
dynamicFriction = FPMath.Max(body1.staticFriction, body2.staticFriction);
restitution = FPMath.Max(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = FPMath.Min(body1.staticFriction, body2.staticFriction);
dynamicFriction = FPMath.Min(body1.staticFriction, body2.staticFriction);
restitution = FPMath.Min(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
dynamicFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
restitution = (body1.restitution + body2.restitution) * FP.Half;
break;
}
}
this.settings = settings;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
FPVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
FPVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
FPVector p1, p2, dp;
FPVector.Add(ref body1.position, ref r1, out p1);
FPVector.Add(ref body2.position, ref r2, out p2);
FPVector.Subtract(ref p2, ref p1, out dp);
FPVector l = FPVector.Transform(lineNormal, body1.orientation);
l.Normalize();
FPVector t = (p1 - p2) % l;
if (t.sqrMagnitude != FP.Zero)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t; // linearVel Body1
jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1
jacobian[2] = -FP.One * t; // linearVel Body2
jacobian[3] = -FP.One * r2 % t; // angularVel Body2
effectiveMass = body1.inverseMass + body2.inverseMass
+ FPVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ FPVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = FP.One / effectiveMass;
}
bias = -(l % (p2 - p1)).magnitude * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <summary>
/// Constraints a point on a body to be fixed on a line
/// which is fixed on another body.
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="lineStartPointBody1"></param>
/// <param name="lineDirection"></param>
/// <param name="pointBody2"></param>
public PointOnLine(RigidBody body1, RigidBody body2,
FPVector lineStartPointBody1, FPVector pointBody2) : base(body1, body2)
{
FPVector.Subtract(ref lineStartPointBody1, ref body1.position, out localAnchor1);
FPVector.Subtract(ref pointBody2, ref body2.position, out localAnchor2);
FPVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
FPVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
lineNormal = FPVector.Normalize(lineStartPointBody1 - pointBody2);
}
// sort cached points so most isolated points come first
private int SortCachedPoints(ref FPVector realRelPos1, FP pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
FP maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
FP res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res0 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 1)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res1 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 2)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[3].relativePos1, ref contactList[1].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res2 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 3)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[2].relativePos1, ref contactList[1].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res3 = cross.sqrMagnitude;
}
int biggestarea = MaxAxis(res0, res1, res2, res3);
return(biggestarea);
}
/// <summary>
/// Initializes a new instance of the DistanceConstraint class.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="anchor1">The anchor point of the first body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
/// <param name="anchor2">The anchor point of the second body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
public PointPointDistance(RigidBody body1, RigidBody body2, FPVector anchor1, FPVector anchor2)
: base(body1, body2)
{
FPVector.Subtract(ref anchor1, ref body1.position, out localAnchor1);
FPVector.Subtract(ref anchor2, ref body2.position, out localAnchor2);
FPVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
FPVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
distance = (anchor1 - anchor2).magnitude;
}
public override void SupportMapping(ref FPVector direction, out FPVector result)
{
FPVector temp1, temp2 = FPVector.zero;
for (int i = 0; i < shapes.Count; i++)
{
shapes[i].SupportMapping(ref direction, out temp1);
FPVector.Add(ref temp1, ref temp2, out temp2);
}
FPVector.Subtract(ref temp2, ref shifted, out result);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
FPVector dp;
FPVector.Subtract(ref body2.position, ref body1.position, out dp);
FP deltaLength = dp.magnitude - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= FP.Zero)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= FP.Zero)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
FPVector n = dp;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
//jacobian[1] = -FP.One * (r1 % n);
jacobian[1] = FP.One * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[1];
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
FPVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
FPVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
FPVector p1, p2, dp;
FPVector.Add(ref body1.position, ref r1, out p1);
FPVector.Add(ref body2.position, ref r2, out p2);
FPVector.Subtract(ref p2, ref p1, out dp);
FP deltaLength = dp.magnitude;
FPVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ FPVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ FPVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <summary>
/// Calculates the inertia of a box with the sides of the multishape.
/// </summary>
public override void CalculateMassInertia()
{
geomCen = FPVector.zero;
// TODO: calc this right
inertia = FPMatrix.Identity;
FPVector size; FPVector.Subtract(ref boundingBox.max, ref boundingBox.min, out size);
mass = size.x * size.y * size.z;
inertia.M11 = (FP.One / (12 * FP.One)) * mass * (size.y * size.y + size.z * size.z);
inertia.M22 = (FP.One / (12 * FP.One)) * mass * (size.x * size.x + size.z * size.z);
inertia.M33 = (FP.One / (12 * FP.One)) * mass * (size.x * size.x + size.y * size.y);
}
private HingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, FPVector position, FPVector hingeAxis) : base((World)world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
FPVector pos1 = position; FPVector.Add(ref pos1, ref hingeAxis, out pos1);
FPVector pos2 = position; FPVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos1);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
Activate();
}
/// <summary>
/// Create a bounding box appropriate for a child, based on a parents AABox
/// </summary>
/// <param name="aabb"></param>
/// <param name="child"></param>
/// <param name="result"></param>
#region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result)
private void CreateAABox(ref FPBBox aabb, EChild child, out FPBBox result)
{
FPVector dims;
FPVector.Subtract(ref aabb.max, ref aabb.min, out dims);
FPVector.Multiply(ref dims, FP.Half, out dims);
FPVector offset = FPVector.zero;
switch (child)
{
case EChild.PPP: offset = new FPVector(1, 1, 1); break;
case EChild.PPM: offset = new FPVector(1, 1, 0); break;
case EChild.PMP: offset = new FPVector(1, 0, 1); break;
case EChild.PMM: offset = new FPVector(1, 0, 0); break;
case EChild.MPP: offset = new FPVector(0, 1, 1); break;
case EChild.MPM: offset = new FPVector(0, 1, 0); break;
case EChild.MMP: offset = new FPVector(0, 0, 1); break;
case EChild.MMM: offset = new FPVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new FPBBox();
result.min = new FPVector(offset.x * dims.x, offset.y * dims.y, offset.z * dims.z);
FPVector.Add(ref result.min, ref aabb.min, out result.min);
FPVector.Add(ref result.min, ref dims, out result.max);
// expand it just a tiny bit just to be safe!
FP extra = FP.EN5;
FPVector temp; FPVector.Multiply(ref dims, extra, out temp);
FPVector.Subtract(ref result.min, ref temp, out result.min);
FPVector.Add(ref result.max, ref temp, out result.max);
}
private int GetCacheEntry(ref FPVector realRelPos1, FP contactBreakThreshold)
{
FP shortestDist = contactBreakThreshold * contactBreakThreshold;
int size = contactList.Count;
int nearestPoint = -1;
for (int i = 0; i < size; i++)
{
FPVector diffA; FPVector.Subtract(ref contactList[i].relativePos1, ref realRelPos1, out diffA);
FP distToManiPoint = diffA.sqrMagnitude;
if (distToManiPoint < shortestDist)
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return(nearestPoint);
}
public static int FindNearestTrianglePoint(SoftBody sb, int id, ref FPVector point)
{
SoftBody.Triangle triangle = sb.dynamicTree.GetUserData(id);
FPVector p;
p = sb.VertexBodies[triangle.indices.I0].position;
FPVector.Subtract(ref p, ref point, out p);
FP length0 = p.sqrMagnitude;
p = sb.VertexBodies[triangle.indices.I1].position;
FPVector.Subtract(ref p, ref point, out p);
FP length1 = p.sqrMagnitude;
p = sb.VertexBodies[triangle.indices.I2].position;
FPVector.Subtract(ref p, ref point, out p);
FP length2 = p.sqrMagnitude;
if (length0 < length1)
{
if (length0 < length2)
{
return(triangle.indices.I0);
}
else
{
return(triangle.indices.I2);
}
}
else
{
if (length1 < length2)
{
return(triangle.indices.I1);
}
else
{
return(triangle.indices.I2);
}
}
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref FPVector point, ref FPVector normal,
out FPVector point1, out FPVector point2)
{
FPVector mn; FPVector.Negate(ref normal, out mn);
FPVector sA; SupportMapping(body1, shape1, ref mn, out sA);
FPVector sB; SupportMapping(body2, shape2, ref normal, out sB);
FPVector.Subtract(ref sA, ref point, out sA);
FPVector.Subtract(ref sB, ref point, out sB);
FP dot1 = FPVector.Dot(ref sA, ref normal);
FP dot2 = FPVector.Dot(ref sB, ref normal);
FPVector.Multiply(ref normal, dot1, out sA);
FPVector.Multiply(ref normal, dot2, out sB);
FPVector.Add(ref point, ref sA, out point1);
FPVector.Add(ref point, ref sB, out point2);
}
/// <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);
}
FPVector sum = points[0];
FPVector.Add(ref sum, ref points[1], out sum);
FPVector.Add(ref sum, ref points[2], out sum);
FPVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
FPVector.Subtract(ref points[1], ref points[0], out sum);
FPVector.Subtract(ref points[2], ref points[0], out normal);
FPVector.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
{
FPVector diff; FPVector.Subtract(ref c.p1, ref c.p2, out diff);
FP distance = FPVector.Dot(ref diff, ref c.normal);
diff = diff - distance * c.normal;
distance = diff.sqrMagnitude;
// 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;
}
}
}
}
/// <summary>
/// Transforms the bounding box into the space given by orientation and position.
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
/// <param name="result"></param>
internal void InverseTransform(ref FPVector position, ref FPMatrix orientation)
{
FPVector.Subtract(ref max, ref position, out max);
FPVector.Subtract(ref min, ref position, out min);
FPVector center;
FPVector.Add(ref max, ref min, out center);
center.x *= FP.Half; center.y *= FP.Half; center.z *= FP.Half;
FPVector halfExtents;
FPVector.Subtract(ref max, ref min, out halfExtents);
halfExtents.x *= FP.Half; halfExtents.y *= FP.Half; halfExtents.z *= FP.Half;
FPVector.TransposedTransform(ref center, ref orientation, out center);
FPMatrix abs; FPMath.Absolute(ref orientation, out abs);
FPVector.TransposedTransform(ref halfExtents, ref abs, out halfExtents);
FPVector.Add(ref center, ref halfExtents, out max);
FPVector.Subtract(ref center, ref halfExtents, out min);
}
/// <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);
FPVector sum = vecs[0];
FPVector.Add(ref sum, ref vecs[1], out sum);
FPVector.Add(ref sum, ref vecs[2], out sum);
FPVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
FPVector.Subtract(ref vecs[1], ref vecs[0], out sum);
FPVector.Subtract(ref vecs[2], ref vecs[0], out normal);
FPVector.Cross(ref sum, ref normal, out normal);
if (flipNormal)
{
normal.Negate();
}
}
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
//ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB,
//out JVector p1, out JVector p2, out JVector normal)
// {
// VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
// simplexSolver.Reset();
// FP lambda = FP.Zero;
// p1 = p2 = JVector.Zero;
// JVector x1 = position1;
// JVector x2 = position2;
// JVector r = sweptA - sweptB;
// JVector w, v;
// JVector supVertexA;
// JVector rn = JVector.Negate(r);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA);
// JVector supVertexB;
// SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB);
// v = supVertexA - supVertexB;
// bool hasResult = false;
// normal = JVector.Zero;
// FP lastLambda = lambda;
// int maxIter = MaxIterations;
// FP distSq = v.LengthSquared();
// FP epsilon = FP.EN5;
// FP VdotR;
// while ((distSq > epsilon) && (maxIter-- != 0))
// {
// JVector vn = JVector.Negate(v);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA);
// SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB);
// w = supVertexA - supVertexB;
// FP VdotW = JVector.Dot(ref v, ref w);
// if (VdotW > FP.Zero)
// {
// VdotR = JVector.Dot(ref v, ref r);
// if (VdotR >= -JMath.Epsilon)
// {
// simplexSolverPool.GiveBack(simplexSolver);
// return false;
// }
// else
// {
// lambda = lambda - VdotW / VdotR;
// x1 = position1 + lambda * sweptA;
// x2 = position2 + lambda * sweptB;
// w = supVertexA - supVertexB;
// normal = v;
// hasResult = true;
// }
// }
// if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
// if (simplexSolver.Closest(out v))
// {
// distSq = v.LengthSquared();
// normal = v;
// hasResult = true;
// }
// else distSq = FP.Zero;
// }
// simplexSolver.ComputePoints(out p1, out p2);
// if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
// normal.Normalize();
// p1 = p1 - lambda * sweptA;
// p2 = p2 - lambda * sweptB;
// simplexSolverPool.GiveBack(simplexSolver);
// return true;
// }
#endregion
// see: btSubSimplexConvexCast.cpp
/// <summary>
/// Checks if a ray definied through it's origin and direction collides
/// with a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="origin">The origin of the ray.</param>
/// <param name="direction">The direction of the ray.</param>
/// <param name="fraction">The fraction which gives information where at the
/// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
/// <param name="normal">The normal from the ray collision.</param>
/// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
public static bool Raycast(ISupportMappable support, ref FPMatrix orientation, ref FPMatrix invOrientation,
ref FPVector position, ref FPVector origin, ref FPVector direction, out FP fraction, out FPVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = FPVector.zero;
fraction = FP.MaxValue;
FP lambda = FP.Zero;
FPVector r = direction;
FPVector x = origin;
FPVector w, p, v;
FPVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
FPVector.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
FP distSq = v.sqrMagnitude;
FP epsilon = FP.EN6;
FP VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
FPVector.Subtract(ref x, ref p, out w);
FP VdotW = FPVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = FPVector.Dot(ref v, ref r);
if (VdotR >= -FPMath.Epsilon)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
FPVector.Multiply(ref r, lambda, out x);
FPVector.Add(ref origin, ref x, out x);
FPVector.Subtract(ref x, ref p, out w);
normal = v;
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
distSq = v.sqrMagnitude;
}
else
{
distSq = FP.Zero;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
FPVector p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.magnitude / direction.magnitude;
#endregion
if (normal.sqrMagnitude > FPMath.Epsilon * FPMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// Checks if given point is within a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="point">The point to check.</param>
/// <returns>Returns true if the point is within the shape, otherwise false.</returns>
public static bool Pointcast(ISupportMappable support, ref FPMatrix orientation, ref FPVector position, ref FPVector point)
{
FPVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
FPVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
FPVector r; support.SupportCenter(out r);
FPVector.Transform(ref r, ref orientation, out r);
FPVector.Add(ref position, ref r, out r);
FPVector.Subtract(ref point, ref r, out r);
FPVector x = point;
FPVector w, p;
FP VdotR;
FPVector v; FPVector.Subtract(ref x, ref arbitraryPoint, out v);
FP dist = v.sqrMagnitude;
FP epsilon = CollideEpsilon;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
FPVector.Subtract(ref x, ref p, out w);
FP VdotW = FPVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = FPVector.Dot(ref v, ref r);
if (VdotR >= -(FPMath.Epsilon * FPMath.Epsilon))
{
simplexSolverPool.GiveBack(simplexSolver); return(false);
}
else
{
simplexSolver.Reset();
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
dist = v.sqrMagnitude;
}
else
{
dist = FP.Zero;
}
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref FPMatrix orientation1,
ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2,
out FPVector point, out FPVector normal, out FP penetration)
{
// Used variables
FPVector temp1, temp2;
FPVector v01, v02, v0;
FPVector v11, v12, v1;
FPVector v21, v22, v2;
FPVector v31, v32, v3;
FPVector v41 = FPVector.zero, v42 = FPVector.zero, v4 = FPVector.zero;
FPVector mn;
// Initialization of the output
point = normal = FPVector.zero;
penetration = FP.Zero;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
FPVector.Transform(ref v01, ref orientation1, out v01);
FPVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
FPVector.Transform(ref v02, ref orientation2, out v02);
FPVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
FPVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new FPVector(FP.EN4, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
FPVector.Negate(ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
FPVector.Subtract(ref v12, ref v11, out v1);
if (FPVector.Dot(ref v1, ref normal) <= FP.Zero)
{
return(false);
}
// v2 = support perpendicular to v1,v0
FPVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
FPVector.Subtract(ref v1, ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
normal.Normalize();
point = v11;
FPVector.Add(ref point, ref v12, out point);
FPVector.Multiply(ref point, FP.Half, out point);
FPVector.Subtract(ref v12, ref v11, out temp1);
penetration = FPVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
FPVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
FPVector.Subtract(ref v22, ref v21, out v2);
if (FPVector.Dot(ref v2, ref normal) <= FP.Zero)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
FP dist = FPVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > FP.Zero)
{
FPVector.Swap(ref v1, ref v2);
FPVector.Swap(ref v11, ref v21);
FPVector.Swap(ref v12, ref v22);
FPVector.Negate(ref normal, out normal);
UnityEngine.Debug.Log("normal: " + normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
FPVector.Negate(ref normal, out mn);
//UnityEngine.Debug.Log("mn: " + mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
FPVector.Subtract(ref v32, ref v31, out v3);
if (FPVector.Dot(ref v3, ref normal) <= FP.Zero)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
FPVector.Cross(ref v1, ref v3, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v2 = v3;
v21 = v31;
v22 = v32;
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v3, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
FPVector.Cross(ref v3, ref v2, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v1 = v3;
v11 = v31;
v12 = v32;
FPVector.Subtract(ref v3, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
/*
* UnityEngine.Debug.LogError(" ::Start STATE");
* UnityEngine.Debug.Log(temp1 + " " + temp2);
* UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0);
* UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1);
* UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2);
* UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3);
* UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4);
* UnityEngine.Debug.Log( mn);
*
* UnityEngine.Debug.LogError(" ::END STATE");
*/
// Compute normal of the wedge face
FPVector.Subtract(ref v2, ref v1, out temp1);
FPVector.Subtract(ref v3, ref v1, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// Beginer
// UnityEngine.Debug.Log("normal: " + normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
//UnityEngine.Debug.Log("normal: " + normal);
// Compute distance from origin to wedge face
FP d = FPVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
FPVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
FPVector.Subtract(ref v42, ref v41, out v4);
FPVector.Subtract(ref v4, ref v3, out temp1);
FP delta = FPVector.Dot(ref temp1, ref normal);
penetration = FPVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= FP.Zero || phase2 > MaximumIterations)
{
if (hit)
{
FPVector.Cross(ref v1, ref v2, out temp1);
FP b0 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v3, ref v2, out temp1);
FP b1 = FPVector.Dot(ref temp1, ref v0);
FPVector.Cross(ref v0, ref v1, out temp1);
FP b2 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v2, ref v1, out temp1);
FP b3 = FPVector.Dot(ref temp1, ref v0);
FP sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
FPVector.Cross(ref v2, ref v3, out temp1);
b1 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v3, ref v1, out temp1);
b2 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v1, ref v2, out temp1);
b3 = FPVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
FP inv = FP.One / sum;
FPVector.Multiply(ref v01, b0, out point);
FPVector.Multiply(ref v11, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v21, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v31, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v02, b0, out temp2);
FPVector.Add(ref temp2, ref point, out point);
FPVector.Multiply(ref v12, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v22, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v32, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref point, inv * FP.Half, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
//// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//FP d1 = JVector.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//FP d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
//UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0);
FPVector.Cross(ref v4, ref v0, out temp1);
//UnityEngine.Debug.LogError("temp1:"+ temp1);
//Ender
// UnityEngine.Debug.Log("normal: " + normal);
FP dot = FPVector.Dot(ref temp1, ref v1);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 );
dot = FPVector.Dot(ref temp1, ref v2);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v1->v4");
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < v3->v4");
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
// UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 );
dot = FPVector.Dot(ref temp1, ref v3);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v2 => v4");
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < 0 v1 => v4");
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}