/// <summary>
/// Gets the closest point on the segment to the given point.
/// </summary>
/// <param name="p">The point with which to calculate the nearest segment point.</param>
/// <param name="scalar">Returns a value between 0 and 1 indicating the location on the segment nearest the point.</param>
/// <param name="output">Returns the closest point on the segment.</param>
public void ClosestPointTo(ref TSVector p, out FP scalar, out TSVector output)
{
TSVector u, v;
TSVector.Subtract(ref p, ref P1, out v);
TSVector.Subtract(ref P2, ref P1, out u);
scalar = TSVector.Dot(ref u, ref v);
scalar /= u.sqrMagnitude;
if (scalar <= FP.Zero)
{
output = P1;
}
else if (scalar >= FP.One)
{
output = P2;
}
else
{
TSVector.Multiply(ref u, scalar, out output);
TSVector.Add(ref P1, ref output, out output);
}
}
public void DebugDraw(IDebugDrawer drawer)
{
TSVector pos1, pos2, pos3;
for (int i = 0; i < hullPoints.Count; i += 3)
{
pos1 = hullPoints[i + 0];
pos2 = hullPoints[i + 1];
pos3 = hullPoints[i + 2];
TSVector.Transform(pos1, orientation, out pos1);
TSVector.Add(pos1, position, out pos1);
TSVector.Transform(pos2, orientation, out pos2);
TSVector.Add(pos2, position, out pos2);
TSVector.Transform(pos3, orientation, out pos3);
TSVector.Add(pos3, position, out pos3);
drawer.DrawTriangle(pos1, pos2, pos3);
}
}
public LimitedHingeJoint(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis, FP minLimit, FP maxLimit) : base(world, body1, body2, position, hingeAxis)
{
TSVector perpDir = TSVector.up;
if (TSVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = TSVector.right;
}
TSVector sideAxis = TSVector.Cross(hingeAxis, perpDir);
perpDir = TSVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
FP len = 15;
TSVector hingeRelAnchorPos0 = perpDir * len;
FP angleToMiddle = FP.Half * (minLimit - maxLimit);
TSMatrix outMatrix;
TSMatrix.CreateFromAxisAngle(ref hingeAxis, -angleToMiddle * FP.Deg2Rad, out outMatrix);
TSVector hingeRelAnchorPos1 = TSVector.Transform(hingeRelAnchorPos0, outMatrix);
FP hingeHalfAngle = FP.Half * (minLimit + maxLimit);
FP allowedDistance = len * 2 * FP.Sin(hingeHalfAngle * FP.Half * FP.Deg2Rad);
TSVector hingePos = body1.TSPosition;
TSVector relPos0c = hingePos + hingeRelAnchorPos0;
TSVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
StateTracker.AddTracking(distance);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
// calculate opposite force needed compare to velocity with respect to jacobian direction vel normalized.
//FP body1VelNeeded = body1.linearVelocity * jacobian[0] * -effectiveMass;
//FP body1AngularVelNeeded = body1.angularVelocity * jacobian[1] * -effectiveMass * j;
//FP body2VelNeeded = body2.linearVelocity * jacobian[2] * -effectiveMass;
//FP body2AngularVelNeeded = body2.angularVelocity * jacobian[3] * -effectiveMass;
// total resisit value needed.
FP jv = body1.linearVelocity * jacobian[0] + body2.linearVelocity * jacobian[2];
//UnityEngine.Debug.Log("effective mass " + effectiveMass);
//FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = jv + bias;
// accumulatedImpulse += -effectiveMass * lambda;
if (!body1.isStatic)
{
//body1.linearVelocity = TSVector.zero;
body1.linearVelocity = (body1.linearVelocity - body2.linearVelocity) * effectiveMass;
//body1.angularVelocity = TSVector.zero;
body1.angularVelocity += TSVector.Transform((lambda) * -effectiveMass * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
//body2.linearVelocity += body2.inverseMass * -effectiveMass * (lambda) * jacobian[2];
//body2.linearVelocity = TSVector.zero;
body2.linearVelocity = (body2.linearVelocity - body1.linearVelocity) * effectiveMass;
//body2.angularVelocity = TSVector.zero;
body2.angularVelocity += TSVector.Transform((lambda) * -effectiveMass * jacobian[3], body2.invInertiaWorld);
}
}
public override void SetDesiredPosOrVelocity(FP deltaTime, IsPosWalkable w = null, bool bSkipBoids = false)
{
bool bValidTargetPos = (_targetPos != TSVector.MinValue && _targetPos != TSVector.MaxValue);
if (!bValidTargetPos)
{
return;
}
TSVector vec = _behaviour.position;//
FP fDtSecond = deltaTime;
//List<IAgentBehaviour> agents = _behaviour.neighbours;
int preBoidsType = _activeBoids;
bool bUsePreBoidsForce = bSkipBoids && _preBoidsForce != TSVector.zero;
_activeBoids = !bUsePreBoidsForce ? _activeBoids : (byte)EBoidsActiveType.none;
// bool bStatic = false;
//TSVector a = ApplyForce(deltaTime,_targetPos-_behaviour.position , _behaviour.velocity, vec, null
// , agents, bUsePreBoidsForce, false); //TSVector.zero;//
//TSVector deltaV = a * fDtSecond;
//FP maxSpeedSqr = _behaviour.maxSpeed * _behaviour.maxSpeed;
//_behaviour.preVelocity = (_behaviour.velocity + deltaV);//.normalized*_behaviour.maxSpeed;
//if (_behaviour.preVelocity.sqrMagnitude > _behaviour.maxSpeed * _behaviour.maxSpeed)//|| TSVector.Dot(_behaviour.velocity, a) > 0
//{
// _behaviour.preVelocity = CustomMath.Normalize(_behaviour.preVelocity) * _behaviour.maxSpeed;
//}
_behaviour.preVelocity = (_targetPos - _behaviour.position).normalized * _behaviour.maxSpeed;
#if UNITY_EDITOR && !MULTI_THREAD
if (PathFindingManager.DEBUG)
{
if (FP.Abs(_behaviour.preVelocity.x) > _behaviour.maxSpeed * 2 || FP.Abs(_behaviour.preVelocity.z) > _behaviour.maxSpeed * 2)
{
UnityEngine.Debug.LogError("velocity error!");
}
}
#endif
_activeBoids = preBoidsType;
}
/// <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);
}
TSVector sum = points[0];
TSVector.Add(sum, points[1], out sum);
TSVector.Add(sum, points[2], out sum);
TSVector.Multiply(sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
TSVector.Subtract(points[1], points[0], out sum);
TSVector.Subtract(points[2], points[0], out normal);
TSVector.Cross(sum, 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
{
TSVector diff; TSVector.Subtract(ref c.p1, ref c.p2, out diff);
FP distance = TSVector.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>
/// Gets the squared distance between the given point and the segment.
/// </summary>
/// <param name="p">The point with which to calculate the distance.</param>
/// <returns>Returns the squared distance.</returns>
public FP DistanceSquaredTo(ref TSVector p)
{
TSVector pq, pp, qp;
FP e, f;
TSVector.Subtract(ref P2, ref P1, out pq);
TSVector.Subtract(ref p, ref P1, out pp);
TSVector.Subtract(ref p, ref P2, out qp);
e = TSVector.Dot(ref pp, ref pq);
if (e <= FP.Zero)
{
return(pp.sqrMagnitude);
}
f = TSVector.Dot(ref pq, ref pq);
if (e >= f)
{
return(qp.sqrMagnitude);
}
return(pp.sqrMagnitude - e * e / f);
}
/// <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 TSVector direction, out TSVector result)
{
FP r = FP.Sqrt(direction.x * direction.x + direction.z * direction.z);
if (FP.Abs(direction.y) > FP.Zero)
{
TSVector dir; TSVector.Normalize(direction, out dir);
TSVector.Multiply(dir, radius, out result);
result.y += FP.Sign(direction.y) * FP.Half * length;
}
else if (r > FP.Zero)
{
result.x = direction.x / r * radius;
result.y = FP.Zero;
result.z = direction.z / r * radius;
}
else
{
result.x = FP.Zero;
result.y = FP.Zero;
result.z = FP.Zero;
}
}
public bool HasNeighboursBetween2Point(int excludeProxyId, TSVector pos1, TSVector pos2, Stack <int> queryStack)
{
#if USING_DYNAMIC_TREE
TSVector2 vec1 = CustomMath.TSVecToVec2(pos1);
TSVector2 vec2 = CustomMath.TSVecToVec2(pos2);
if (_dynamicTree != null)
{
RayCastInput rc = new RayCastInput();
rc.p1 = vec1;
rc.p2 = vec2;
rc.maxFraction = 1;// GridMap.nodeSize * FP.EN1 * 5;/// 20*nodeSize;
int proxyId = _dynamicTree.RayCast(null, ref rc, queryStack, true, excludeProxyId);
if (proxyId >= 0)
{
// PathFindingAgentBehaviour blockedAgent = _dynamicTree.GetUserData(proxyId);
return(true);
}
}
return(false);
#else
return(false);
#endif
}
internal FlowField UpdateAgent(IAgentBehaviour agent, TSVector preDest, TSVector dest, GridMap map)
{
if (preDest != dest)
{
FlowField ff;
bool hasPre = preDest != TSVector.MinValue;
int idx = map.GetGridNodeId(preDest);
if (hasPre && _dicFlowFields.TryGetValue(idx, out ff))
{
ff.RemoveAgent(agent);
}
idx = map.GetGridNodeId(dest);
if (!_dicFlowFields.TryGetValue(idx, out ff))
{
ff = new FlowField(dest, map);
_dicFlowFields[idx] = ff;
}
ff.AddAgent(agent);
return(ff);
}
return(null);
}
/// <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 TSVector direction, out TSVector result)
{
FP sigma = FP.Sqrt(direction.x * direction.x + direction.z * direction.z);
if (direction.y > direction.magnitude * sina)
{
result.x = FP.Zero;
result.y = ((2 * FP.One) / (3)) * height;
result.z = FP.Zero;
}
else if (sigma > FP.Zero)
{
result.x = radius * direction.x / sigma;
result.y = -(FP.One / 3) * height;
result.z = radius * direction.z / sigma;
}
else
{
result.x = FP.Zero;
result.y = -(FP.One / 3) * height;
result.z = FP.Zero;
}
}
internal GameObject Pop(TSVector position, TSQuaternion rotation)
{
GameObject returnobj = null;
if (mGameList.Count > 0)
{
returnobj = mGameList[mGameList.Count - 1];
mGameList.RemoveAt(mGameList.Count - 1);
}
else
{
if (mAssetObj == null)
{
mAssetObj = _AssetManager.GetGameObject(mPath);
mAssetObj.transform.SetParent(mRootTrans);
mAssetObj.SetActive(false);
}
returnobj = TrueSyncManager.SyncedInstantiate(mAssetObj, position, rotation);
}
returnobj.transform.SetParent(mRootTrans);
returnobj.SetActive(true);
return(returnobj);
}
private void AddPressureForces(FP timeStep)
{
if (pressure == FP.Zero || volume == FP.Zero)
{
return;
}
FP invVolume = FP.One / volume;
foreach (Triangle t in triangles)
{
TSVector v1 = points[t.indices.I0].position;
TSVector v2 = points[t.indices.I1].position;
TSVector v3 = points[t.indices.I2].position;
TSVector cross = (v3 - v1) % (v2 - v1);
//TSVector center = (v1 + v2 + v3) * (FP.One / (3 * FP.One));
points[t.indices.I0].AddForce(invVolume * cross * pressure);
points[t.indices.I1].AddForce(invVolume * cross * pressure);
points[t.indices.I2].AddForce(invVolume * cross * pressure);
}
}
private void IntegrateForces()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
if (!body.isStatic && body.IsActive)
{
TSVector temp;
TSVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
TSVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
if (!(body.isParticle))
{
TSVector.Multiply(ref body.torque, timestep, out temp);
TSVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
TSVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
//clamp angular velocity to 0.5pi .
FP maxAngVel = TSMath.PiOver2;
FP angvel = body.angularVelocity.magnitude;
if (angvel * timestep > maxAngVel)
{
body.angularVelocity *= (maxAngVel / timestep) / angvel;
}
}
if (body.affectedByGravity)
{
TSVector.Multiply(ref gravity, timestep, out temp);
TSVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
/// <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);
TSVector sum = vecs[0];
TSVector.Add(ref sum, ref vecs[1], out sum);
TSVector.Add(ref sum, ref vecs[2], out sum);
TSVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
TSVector.Subtract(ref vecs[1], ref vecs[0], out sum);
TSVector.Subtract(ref vecs[2], ref vecs[0], out normal);
TSVector.Cross(ref sum, ref normal, out normal);
normal.Normalize();
if (flipNormal)
{
normal.Negate();
}
}
/// <summary>
/// Creates a matrix which rotates around the given axis by the given angle.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="angle">The angle.</param>
/// <param name="result">The resulting rotation matrix</param>
#region public static void CreateFromAxisAngle(ref JVector axis, FP angle, out JMatrix result)
public static void CreateFromAxisAngle(ref TSVector axis, FP angle, out TSMatrix result)
{
FP x = axis.x;
FP y = axis.y;
FP z = axis.z;
FP num2 = FP.Sin(angle);
FP num = FP.Cos(angle);
FP num11 = x * x;
FP num10 = y * y;
FP num9 = z * z;
FP num8 = x * y;
FP num7 = x * z;
FP num6 = y * z;
result.M11 = num11 + (num * (FP.One - num11));
result.M12 = (num8 - (num * num8)) + (num2 * z);
result.M13 = (num7 - (num * num7)) - (num2 * y);
result.M21 = (num8 - (num * num8)) - (num2 * z);
result.M22 = num10 + (num * (FP.One - num10));
result.M23 = (num6 - (num * num6)) + (num2 * x);
result.M31 = (num7 - (num * num7)) + (num2 * y);
result.M32 = (num6 - (num * num6)) - (num2 * x);
result.M33 = num9 + (num * (FP.One - num9));
}
/// <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 TSVector position, ref TSMatrix orientation)
{
TSVector.Subtract(ref max, ref position, out max);
TSVector.Subtract(ref min, ref position, out min);
TSVector center;
TSVector.Add(ref max, ref min, out center);
center.x *= FP.Half; center.y *= FP.Half; center.z *= FP.Half;
TSVector halfExtents;
TSVector.Subtract(ref max, ref min, out halfExtents);
halfExtents.x *= FP.Half; halfExtents.y *= FP.Half; halfExtents.z *= FP.Half;
TSVector.TransposedTransform(ref center, ref orientation, out center);
TSMatrix abs; TSMath.Absolute(ref orientation, out abs);
TSVector.TransposedTransform(ref halfExtents, ref abs, out halfExtents);
TSVector.Add(ref center, ref halfExtents, out max);
TSVector.Subtract(ref center, ref halfExtents, out min);
}
//
public void IniFlowField(int w, int h, TSVector destination)//destination from
{
#if USING_ASTAR_PROJECT && UNITY_EDITOR
_gridWidth = Pathfinding.GridNode.GetGridGraph(0).width;
_gridHeight = Pathfinding.GridNode.GetGridGraph(0).depth;
#else
_gridWidth = w;
_gridHeight = h;
#endif
_destination = destination;
if (_gridDijkstraCost == null)
{
_gridDijkstraCost = new FP[_gridWidth, _gridHeight];
}
if (_flowField == null)
{
_flowField = new TSVector[_gridWidth, _gridHeight];
}
if (_gridLOS == null)
{
_gridLOS = new bool[_gridWidth, _gridHeight];
}
UpdateFlowField();
}
public void ApplyImpulse(TSVector impulse)
{
#region INLINE - HighFrequency
//JVector temp;
if (!treatBody1AsStatic)
{
body1.linearVelocity.x -= (impulse.x * body1.inverseMass);
body1.linearVelocity.y -= (impulse.y * body1.inverseMass);
body1.linearVelocity.z -= (impulse.z * body1.inverseMass);
FP num0, num1, num2;
num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y;
num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z;
num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x;
FP num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
FP num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
FP num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.x -= num3;
body1.angularVelocity.y -= num4;
body1.angularVelocity.z -= num5;
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.x += (impulse.x * body2.inverseMass);
body2.linearVelocity.y += (impulse.y * body2.inverseMass);
body2.linearVelocity.z += (impulse.z * body2.inverseMass);
FP num0, num1, num2;
num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y;
num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z;
num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x;
FP num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
FP num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
FP num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.x += num3;
body2.angularVelocity.y += num4;
body2.angularVelocity.z += num5;
}
#endregion
}
public bool ClosestPtPointTetrahedron(TSVector a, TSVector b, TSVector c, TSVector d,
ref SubSimplexClosestResult finalResult)
{
tempResult.Reset();
// Start out assuming point inside all halfspaces, so closest to itself
finalResult.closestPointOnSimplex = TSVector.zero;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = true;
finalResult.usedVertices.UsedVertexB = true;
finalResult.usedVertices.UsedVertexC = true;
finalResult.usedVertices.UsedVertexD = true;
int pointOutsideABC = PointOutsideOfPlane(a, b, c, d);
int pointOutsideACD = PointOutsideOfPlane(a, c, d, b);
int pointOutsideADB = PointOutsideOfPlane(a, d, b, c);
int pointOutsideBDC = PointOutsideOfPlane(b, d, c, a);
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
{
finalResult.degenerate = true;
return(false);
}
if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
{
return(false);
}
FP bestSqDist = FP.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
ClosestPtPointTriangle(a, b, c, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
FP sqDist = q.sqrMagnitude;
// Update best closest point if (squared) distance is less than current best
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
//convert result bitmask!
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
tempResult.barycentricCoords[VertexA],
tempResult.barycentricCoords[VertexB],
tempResult.barycentricCoords[VertexC],
FP.Zero);
}
}
// Repeat test for face acd
if (pointOutsideACD != 0)
{
ClosestPtPointTriangle(a, c, d, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
//convert result bitmask!
FP sqDist = q.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
tempResult.barycentricCoords[VertexA],
FP.Zero,
tempResult.barycentricCoords[VertexB],
tempResult.barycentricCoords[VertexC]);
}
}
// Repeat test for face adb
if (pointOutsideADB != 0)
{
ClosestPtPointTriangle(a, d, b, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
//convert result bitmask!
FP sqDist = q.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
tempResult.barycentricCoords[VertexA],
tempResult.barycentricCoords[VertexC],
FP.Zero,
tempResult.barycentricCoords[VertexB]);
}
}
// Repeat test for face bdc
if (pointOutsideBDC != 0)
{
ClosestPtPointTriangle(b, d, c, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
//convert result bitmask!
FP sqDist = q.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
FP.Zero,
tempResult.barycentricCoords[VertexA],
tempResult.barycentricCoords[VertexC],
tempResult.barycentricCoords[VertexB]);
}
}
//help! we ended up full !
if (finalResult.usedVertices.UsedVertexA &&
finalResult.usedVertices.UsedVertexB &&
finalResult.usedVertices.UsedVertexC &&
finalResult.usedVertices.UsedVertexD)
{
return(true);
}
return(true);
}
public bool ClosestPtPointTriangle(TSVector a, TSVector b, TSVector c,
ref SubSimplexClosestResult result)
{
result.usedVertices.Reset();
FP v, w;
// Check if P in vertex region outside A
TSVector ab = b - a;
TSVector ac = c - a;
FP d1 = TSVector.Dot(ab, a);
FP d2 = TSVector.Dot(ac, a);
if (d1 >= FP.Zero && d2 >= FP.Zero)
{
result.closestPointOnSimplex = a;
result.usedVertices.UsedVertexA = true;
result.SetBarycentricCoordinates(FP.One, FP.Zero, FP.Zero, FP.Zero);
return(true); // a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
FP d3 = TSVector.Dot(ab, b);
FP d4 = TSVector.Dot(ac, b);
if (d3 <= FP.Zero && d4 >= d3)
{
result.closestPointOnSimplex = b;
result.usedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(FP.Zero, FP.One, FP.Zero, FP.Zero);
return(true); // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
FP vc = d1 * d4 - d3 * d2;
if (vc <= FP.Zero && d1 >= FP.Zero && d3 <= FP.Zero)
{
v = d1 / (d1 - d3);
result.closestPointOnSimplex = a + v * ab;
result.usedVertices.UsedVertexA = true;
result.usedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(FP.One - v, v, FP.Zero, FP.Zero);
return(true);
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
FP d5 = TSVector.Dot(ab, c);
FP d6 = TSVector.Dot(ac, c);
if (d6 <= FP.Zero && d5 >= d6)
{
result.closestPointOnSimplex = c;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.Zero, FP.Zero, FP.One, FP.Zero);
return(true);//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
FP vb = d5 * d2 - d1 * d6;
if (vb <= FP.Zero && d2 >= FP.Zero && d6 <= FP.Zero)
{
w = d2 / (d2 - d6);
result.closestPointOnSimplex = a + w * ac;
result.usedVertices.UsedVertexA = true;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.One - w, FP.Zero, w, FP.Zero);
return(true);
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
FP va = d3 * d6 - d5 * d4;
FP d34 = d4 - d3;
FP d65 = d5 - d6;
if (va <= FP.Zero && d34 >= FP.Zero && d65 >= FP.Zero)
{
w = d34 / (d34 + d65);
result.closestPointOnSimplex = b + w * (c - b);
result.usedVertices.UsedVertexB = true;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.Zero, FP.One - w, w, FP.Zero);
return(true);
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
FP denom = FP.One / (va + vb + vc);
v = vb * denom;
w = vc * denom;
result.closestPointOnSimplex = a + ab * v + ac * w;
result.usedVertices.UsedVertexA = true;
result.usedVertices.UsedVertexB = true;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.One - v - w, v, w, FP.Zero);
return(true);
}
public void Do(int skillId, uint targetAgentId, AgentObjectType targetAgentType, TSVector position, TSVector forward)
{
Do(GetSkill(skillId), targetAgentId, targetAgentType, position, forward);
}
public void ComputePoints(out TSVector p1, out TSVector p2)
{
UpdateClosestVectorAndPoints();
p1 = _cachedPA;
p2 = _cachedPB;
}
public void BackupClosest(out TSVector v)
{
v = _cachedV;
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
FP dvx, dvy, dvz;
dvx = (body2.angularVelocity.y * relativePos2.z) - (body2.angularVelocity.z * relativePos2.y) + body2.linearVelocity.x;
dvy = (body2.angularVelocity.z * relativePos2.x) - (body2.angularVelocity.x * relativePos2.z) + body2.linearVelocity.y;
dvz = (body2.angularVelocity.x * relativePos2.y) - (body2.angularVelocity.y * relativePos2.x) + body2.linearVelocity.z;
dvx = dvx - (body1.angularVelocity.y * relativePos1.z) + (body1.angularVelocity.z * relativePos1.y) - body1.linearVelocity.x;
dvy = dvy - (body1.angularVelocity.z * relativePos1.x) + (body1.angularVelocity.x * relativePos1.z) - body1.linearVelocity.y;
dvz = dvz - (body1.angularVelocity.x * relativePos1.y) + (body1.angularVelocity.y * relativePos1.x) - body1.linearVelocity.z;
FP kNormal = FP.Zero;
TSVector rantra = TSVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * normal.z) - (relativePos1.z * normal.y);
rantra.y = (relativePos1.z * normal.x) - (relativePos1.x * normal.z);
rantra.z = (relativePos1.x * normal.y) - (relativePos1.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
TSVector rbntrb = TSVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * normal.z) - (relativePos2.z * normal.y);
rbntrb.y = (relativePos2.z * normal.x) - (relativePos2.x * normal.z);
rbntrb.z = (relativePos2.x * normal.y) - (relativePos2.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kNormal += rantra.x * normal.x + rantra.y * normal.y + rantra.z * normal.z;
}
if (!treatBody2AsStatic)
{
kNormal += rbntrb.x * normal.x + rbntrb.y * normal.y + rbntrb.z * normal.z;
}
massNormal = FP.One / kNormal;
FP num = dvx * normal.x + dvy * normal.y + dvz * normal.z;
tangent.x = dvx - normal.x * num;
tangent.y = dvy - normal.y * num;
tangent.z = dvz - normal.z * num;
num = tangent.x * tangent.x + tangent.y * tangent.y + tangent.z * tangent.z;
if (num != FP.Zero)
{
num = FP.Sqrt(num);
tangent.x /= num;
tangent.y /= num;
tangent.z /= num;
}
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * tangent.z) - (relativePos1.z * tangent.y);
rantra.y = (relativePos1.z * tangent.x) - (relativePos1.x * tangent.z);
rantra.z = (relativePos1.x * tangent.y) - (relativePos1.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * tangent.z) - (relativePos2.z * tangent.y);
rbntrb.y = (relativePos2.z * tangent.x) - (relativePos2.x * tangent.z);
rbntrb.z = (relativePos2.x * tangent.y) - (relativePos2.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kTangent += TSVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += TSVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = normal.x * dvx + normal.y * dvy + normal.z * dvz; //JVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -(tangent.x * dvx + tangent.y * dvy + tangent.z * dvz);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
TSVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
// modified by tiger, uncommmented.
//if (relNormalVel < -FP.One && newContact)
//{
// restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
//}
// added by seok
//if (!newContact)
if (!newContact || TSMath.Abs(relNormalVel *= restitution) < restitution)
{
relNormalVel = 0;
}
restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse.x = normal.x * accumulatedNormalImpulse + tangent.x * accumulatedTangentImpulse;
impulse.y = normal.y * accumulatedNormalImpulse + tangent.y * accumulatedTangentImpulse;
impulse.z = normal.z * accumulatedNormalImpulse + tangent.z * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.x -= (impulse.x * body1.inverseMass);
body1.linearVelocity.y -= (impulse.y * body1.inverseMass);
body1.linearVelocity.z -= (impulse.z * body1.inverseMass);
if (!body1IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y;
num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z;
num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x;
FP num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
FP num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
FP num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.x -= num3;
body1.angularVelocity.y -= num4;
body1.angularVelocity.z -= num5;
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.x += (impulse.x * body2.inverseMass);
body2.linearVelocity.y += (impulse.y * body2.inverseMass);
body2.linearVelocity.z += (impulse.z * body2.inverseMass);
if (!body2IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y;
num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z;
num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x;
FP num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
FP num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
FP num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.x += num3;
body2.angularVelocity.y += num4;
body2.angularVelocity.z += num5;
}
}
lastTimeStep = timestep;
newContact = false;
}
/// <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 TSVector point1, ref TSVector point2, ref TSVector 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;
TSVector.Subtract(ref p1, ref body1.position, out relativePos1);
TSVector.Subtract(ref p2, ref body2.position, out relativePos2);
TSVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
TSVector.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 = TSMath.Max(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Max(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
restitution = TSMath.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;
}
public bool UpdateClosestVectorAndPoints()
{
if (_needsUpdate)
{
_cachedBC.Reset();
_needsUpdate = false;
TSVector p, a, b, c, d;
switch (NumVertices)
{
case 0:
_cachedValidClosest = false;
break;
case 1:
_cachedPA = _simplexPointsP[0];
_cachedPB = _simplexPointsQ[0];
_cachedV = _cachedPA - _cachedPB;
_cachedBC.Reset();
_cachedBC.SetBarycentricCoordinates(FP.One, FP.Zero, FP.Zero, FP.Zero);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 2:
//closest point origin from line segment
TSVector from = _simplexVectorW[0];
TSVector to = _simplexVectorW[1];
//TSVector nearest;
TSVector diff = TSVector.Negate(from);
TSVector v = to - from;
FP t = TSVector.Dot(v, diff);
if (t > FP.Zero)
{
FP dotVV = v.sqrMagnitude;
if (t < dotVV)
{
t /= dotVV;
diff -= t * v;
_cachedBC.usedVertices.UsedVertexA = true;
_cachedBC.usedVertices.UsedVertexB = true;
}
else
{
t = FP.One;
diff -= v;
//reduce to 1 point
_cachedBC.usedVertices.UsedVertexB = true;
}
}
else
{
t = FP.Zero;
//reduce to 1 point
_cachedBC.usedVertices.UsedVertexA = true;
}
_cachedBC.SetBarycentricCoordinates(FP.One - t, t, FP.Zero, FP.Zero);
//nearest = from + t * v;
_cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
_cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.usedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 3:
//closest point origin from triangle
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
ClosestPtPointTriangle(a, b, c, ref _cachedBC);
_cachedPA = _simplexPointsP[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.barycentricCoords[2];
_cachedPB = _simplexPointsQ[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.barycentricCoords[2];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.usedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 4:
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
d = _simplexVectorW[3];
bool hasSeparation = ClosestPtPointTetrahedron(a, b, c, d, ref _cachedBC);
if (hasSeparation)
{
_cachedPA = _simplexPointsP[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.barycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.barycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.barycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.barycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.barycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.barycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.usedVertices);
}
else
{
if (_cachedBC.degenerate)
{
_cachedValidClosest = false;
}
else
{
_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
_cachedV.x = _cachedV.y = _cachedV.z = FP.Zero;
}
break; // !!!!!!!!!!!! proverit na vsakiy sluchai
}
_cachedValidClosest = _cachedBC.IsValid;
//closest point origin from tetrahedron
break;
default:
_cachedValidClosest = false;
break;
}
}
return(_cachedValidClosest);
}
public void Do(Skill skill, uint targetAgentId, AgentObjectType targetAgentType, TSVector position, TSVector forward)
{
if (skill == null)
{
return;
}
if (!CanDo(skill))
{
return;
}
m_lstCurSkill.Add(skill);
skill.Do(targetAgentId, targetAgentType, position, forward);
}
/// <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 TSMatrix orientation, ref TSVector position, ref TSVector point)
{
TSVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
TSVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
TSVector r; support.SupportCenter(out r);
TSVector.Transform(ref r, ref orientation, out r);
TSVector.Add(ref position, ref r, out r);
TSVector.Subtract(ref point, ref r, out r);
TSVector x = point;
TSVector w, p;
FP VdotR;
TSVector v; TSVector.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);
TSVector.Subtract(ref x, ref p, out w);
FP VdotW = TSVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = TSVector.Dot(ref v, ref r);
if (VdotR >= -(TSMath.Epsilon * TSMath.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);
}
