/// <summary>
/// See also geometrictools.com
/// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="p"></param>
/// <param name="nearest"></param>
/// <returns></returns>
private FP SegmentSquareDistance(TSVector from, TSVector to, TSVector point, ref TSVector nearest)
{
TSVector diff = point - from;
TSVector v = to - from;
FP t = TSVector.Dot(v, diff);
if (t > FP.Zero)
{
FP dotVV = TSVector.Dot(v, v);
if (t < dotVV)
{
t /= dotVV;
diff -= t * v;
}
else
{
t = 1;
diff -= v;
}
}
else
{
t = 0;
}
nearest = from + t * v;
return(TSVector.Dot(diff, diff));
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref TSVector point, ref TSVector normal,
out TSVector point1, out TSVector point2)
{
TSVector mn;
TSVector.Negate(ref normal, out mn);
TSVector sA;
SupportMapping(body1, shape1, ref mn, out sA);
TSVector sB;
SupportMapping(body2, shape2, ref normal, out sB);
TSVector.Subtract(ref sA, ref point, out sA);
TSVector.Subtract(ref sB, ref point, out sB);
FP dot1 = TSVector.Dot(ref sA, ref normal);
FP dot2 = TSVector.Dot(ref sB, ref normal);
TSVector.Multiply(ref normal, dot1, out sA);
TSVector.Multiply(ref normal, dot2, out sB);
TSVector.Add(ref point, ref sA, out point1);
TSVector.Add(ref point, ref sB, out point2);
}
public void SupportMapping(ref TSVector direction, out TSVector result)
{
FP min = TSVector.Dot(owner.points[indices.I0].position, direction);
FP dot = TSVector.Dot(owner.points[indices.I1].position, direction);
TSVector minVertex = owner.points[indices.I0].position;
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I1].position;
}
dot = TSVector.Dot(owner.points[indices.I2].position, direction);
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I2].position;
}
TSVector exp;
TSVector.Normalize(direction, out exp);
exp *= owner.triangleExpansion;
result = minVertex + exp;
}
/// <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)
{
TSVector expandVector;
TSVector.Normalize(direction, out expandVector);
TSVector.Multiply(expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(points[0], direction);
FP dot = TSVector.Dot(points[1], direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = TSVector.Dot(points[2], direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
TSVector.Add(points[minIndex], expandVector, out result);
}
/// <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)
{
TSVector exp;
TSVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
FP min = TSVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
FP dot = TSVector.Dot(ref vecs[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = TSVector.Dot(ref vecs[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
result = vecs[minIndex] + exp;
}
private bool PointInTriangle(ref TSVector[] vertices, ref TSVector normal, ref TSVector p)
{
TSVector p1 = vertices[0];
TSVector p2 = vertices[1];
TSVector p3 = vertices[2];
TSVector edge1 = p2 - p1;
TSVector edge2 = p3 - p2;
TSVector edge3 = p1 - p3;
TSVector p1ToP = p - p1;
TSVector p2ToP = p - p2;
TSVector p3ToP = p - p3;
TSVector edge1Normal = TSVector.Cross(edge1, normal);
TSVector edge2Normal = TSVector.Cross(edge2, normal);
TSVector edge3Normal = TSVector.Cross(edge3, normal);
FP r1, r2, r3;
r1 = TSVector.Dot(edge1Normal, p1ToP);
r2 = TSVector.Dot(edge2Normal, p2ToP);
r3 = TSVector.Dot(edge3Normal, p3ToP);
if ((r1 > FP.Zero && r2 > FP.Zero && r3 > FP.Zero) ||
(r1 <= FP.Zero && r2 <= FP.Zero && r3 <= FP.Zero))
{
return(true);
}
return(false);
}
/// <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)
{
TSVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
TSVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
TSVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsMassPoint)
{
TSVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
TSVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
TSVector.Add(ref p2, ref body2.position, out p2);
}
TSVector dist; TSVector.Subtract(ref p1, ref p2, out dist);
penetration = TSVector.Dot(ref dist, ref normal);
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector w = TSVector.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
if (normal.sqrMagnitude < TSMath.Epsilon)
{
// Spheres are on the same position, we can choose any normal vector.
// Possibly it would be better to consider the object movement (velocities), but
// it is not important since this case should be VERY rare.
normal = TSVector.forward;
}
else
{
normal = normal.normalized;
}
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
return(true);
}
return(false);
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
public TSVector Solve33(TSVector b)
{
FP det = TSVector.Dot(ex, TSVector.Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
return(new TSVector(det * TSVector.Dot(b, TSVector.Cross(ey, ez)), det * TSVector.Dot(ex, TSVector.Cross(b, ez)), det * TSVector.Dot(ex, TSVector.Cross(ey, b))));
}
/// <summary>
/// Avoids the specified units.
/// </summary>
/// <param name="units">The units list.</param>
/// <param name="unitsLength">Length of the units list.</param>
/// <param name="currentVelocity">This unit's current velocity.</param>
/// <returns>An avoid vector, if there are any to avoid, otherwise TSVector.zero.</returns>
private static TSVector Avoid(List <IAgentBehaviour> units, IAgentBehaviour agent, int unitsLength, TSVector currentVelocity)
{
TSVector normalVelocity = CustomMath.Normalize(currentVelocity);
TSVector selfPos = agent.position + normalVelocity;
TSVector combinedAvoidVector = TSVector.zero;
// iterate through scanned units list
for (int i = 0; i < unitsLength; i++)
{
var other = units[i];
if (!other.enabled)
{
continue;
}
//if (_unitData.transientGroup != null && object.ReferenceEquals(other.transientGroup, _unitData.transientGroup))
//{
// // ignore units in same transient unit group
// // continue;
//}
//if (other.determination < _unitData.determination)
//{
// // ignore units with lower determination
// continue;
//}
TSVector otherPos = other.position;
TSVector direction = otherPos - selfPos;
FP distance = direction.magnitude;
FP omniAwareRadius = agent.colliderRadius;
if (distance > omniAwareRadius && TSVector.Dot(normalVelocity, direction / distance) > _fovReverseAngleCos)
{
// the other unit is behind me and outside my 'omni aware radius', ignore it
continue;
}
FP combinedRadius = other.colliderRadius + other.colliderRadius + radiusMargin * GridMap.GetNodeSize();
TSVector otherVelocity = other.velocity;
TSVector avoidVector = GetAvoidVector(selfPos, currentVelocity, normalVelocity, agent, otherPos, otherVelocity, other, combinedRadius);
if (accumulateAvoidVectors)
{
// if accumulating, then keep summing avoid vectors up
combinedAvoidVector += avoidVector;
}
else
{
// if not accumulating, then break after the first avoid vector is found
combinedAvoidVector = avoidVector;
break;
}
}
return(combinedAvoidVector);
}
public static FP CalcK(RigidBody obj, TSVector relativePos, TSVector _normal)
{
if (!obj.IsStatic)
{
var v = TSVector.Cross(relativePos, _normal);
v = obj.invInertiaWorld.TransposedMultiply(v);
v = TSVector.Cross(v, relativePos);
return(obj.inverseMass + TSVector.Dot(v, _normal));
}
return(0);
}
//https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
private bool RayIntersectsTriangle(ISupportMappable support, ref TSMatrix orientation, ref TSMatrix invOrientation,
ref TSVector position, ref TSVector origin, ref TSVector direction, out FP fraction, out TSVector normal)
{
FP EPSILON = FP.EN8;
fraction = FP.Zero;
normal = TSVector.zero;
TriangleMeshShape inTriangle = support as TriangleMeshShape;
TSVector[] vertices = inTriangle.Vertices;
TSVector vertex0 = vertices[0];
TSVector vertex1 = vertices[1];
TSVector vertex2 = vertices[2];
TSVector edge1, edge2, h, s, q;
FP a, f, u, v;
edge1 = inTriangle.edge1;
edge2 = inTriangle.edge2;
h = TSVector.Cross(direction, edge2);
a = TSVector.Dot(edge1, h);
if (a > -EPSILON && a < EPSILON)
{
return(false);
}
f = 1 / a;
s = origin - vertex0;
u = f * (TSVector.Dot(s, h));
if (u < FP.Zero || u > FP.One)
{
return(false);
}
q = TSVector.Cross(s, edge1);
v = f * TSVector.Dot(direction, q);
if (v < FP.Zero || u + v > FP.One)
{
return(false);
}
// At this stage we can compute t to find out where the intersection point is on the line.
fraction = f * TSVector.Dot(edge2, q);
if (fraction > EPSILON) // ray intersection
{
return(true);
}
else // This means that there is a line intersection but not a ray intersection.
{
return(false);
}
}
public static TSVector ClosestPointOnSegment(TSVector lineStart, TSVector lineEnd, TSVector point)
{
var dir = lineEnd - lineStart;
FP sqrMagn = dir.sqrMagnitude;
if (sqrMagn <= EPSILON)
{
return(lineStart);
}
FP factor = TSVector.Dot(point - lineStart, dir) / sqrMagn;
return(lineStart + TSMath.Clamp(factor, 0, 1) * dir);
}
public bool IsWalkableSkipDynamic(TSVector pos)
{
int idx = flowField._gridMap.GetGridNodeId(pos);
if (flowField._gridMap.IsDynamicUnwalkableNode(idx))
{
TSVector centerPos = flowField._gridMap.GetWorldPosition(idx);
FP dot = TSVector.Dot(pos - _behaviour.position, centerPos - _behaviour.position);
return(dot <= 0);// CustomMath.FPHalf;//>=60 degree
}
IInt2 iPos = flowField._gridMap.GetNearestGridCoordWithoutClamp(pos);
return(flowField._gridMap.IsWalkable(iPos.x, iPos.y, true));
}
/// <summary>
/// is the target infront of ship.
/// </summary>
/// <param name="targetpos">the target to check with</param>
/// <returns></returns>
bool FrontTest()
{
TSVector fwd = transformts.forward;
TSVector vec = localtarget - transformts.position;
vec.Normalize();
FP ang = TSMath.Acos(TSVector.Dot(fwd, vec)) * Mathf.Rad2Deg;
if (ang <= 45.0f)
{
return(true);
}
return(false);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = TSVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += TSVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
TSVector temp;
CBFrame.Utils.Logger.Debug("line195 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
TSVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
TSVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line201 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
TSVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
TSVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line206 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// Test if point p and d lie on opposite sides of plane through abc
public int PointOutsideOfPlane(TSVector p, TSVector a, TSVector b, TSVector c, TSVector d)
{
TSVector normal = TSVector.Cross(b - a, c - a);
FP signp = TSVector.Dot(p - a, normal); // [AP AB AC]
FP signd = TSVector.Dot(d - a, normal); // [AD AB AC]
//if (CatchDegenerateTetrahedron)
if (signd * signd < (FP.EN8))
{
return(-1);
}
// Points on opposite sides if expression signs are opposite
return(signp * signd < FP.Zero ? 1 : 0);
}
/// <summary>
/// Gets an avoidance vector.
/// </summary>
/// <param name="selfPos">This unit's position.</param>
/// <param name="currentVelocity">This unit's current velocity.</param>
/// <param name="normalVelocity">This unit's normalized current velocity.</param>
/// <param name="unitData">This unit's UnitFacade.</param>
/// <param name="otherPos">The other unit's position.</param>
/// <param name="otherVelocity">The other unit's velocity.</param>
/// <param name="otherData">The other unit's UnitFacade.</param>
/// <param name="combinedRadius">The combined radius.</param>
/// <returns>An avoidance vector from the other unit's collision position to this unit's collision position - if a collision actually is detected.</returns>
private static TSVector GetAvoidVector(TSVector selfPos, TSVector currentVelocity, TSVector normalVelocity, IAgentBehaviour unitData, TSVector otherPos, TSVector otherVelocity, IAgentBehaviour otherData, FP combinedRadius)
{
TSVector selfCollisionPos = TSVector.zero;
TSVector avoidDirection = GetAvoidDirectionVector(selfPos, currentVelocity, otherPos, otherVelocity, combinedRadius, out selfCollisionPos);
FP avoidMagnitude = avoidDirection.magnitude;
if (avoidMagnitude == 0)
{
// if there is absolutely no magnitude to the found avoid direction, then ignore it
return(TSVector.zero);
}
FP vectorLength = combinedRadius * CustomMath.FPHalf;
if (vectorLength <= 0)
{
// if the units' combined radius is 0, then we cannot avoid
return(TSVector.zero);
}
// normalize the avoid vector and then set it's magnitude to the desired vector length (half of the combined radius)
TSVector avoidNormalized = (avoidDirection / avoidMagnitude);
TSVector avoidVector = avoidNormalized * vectorLength;
FP dotAngle = TSVector.Dot(avoidNormalized, normalVelocity);
if (dotAngle <= _cosAvoidAngle)
{
// the collision is considered "head-on", thus we compute a perpendicular avoid vector instead
avoidVector = new TSVector(avoidVector.z, avoidVector.y, -avoidVector.x);
}
else if (preventPassingInFront
//&& (otherData.determination > unitData.determination)
&& (TSVector.Dot(otherVelocity, avoidVector) > 0 && TSVector.Dot(currentVelocity, otherVelocity) >= 0))
{
// if supposed to be preventing front-passing, then check whether we should prevent it in this case and if so compute a different avoid vector
avoidVector = selfCollisionPos - selfPos;
}
// scale the avoid vector depending on the distance to collision, shorter distances need larger magnitudes and vice versa
FP collisionDistance = TSMath.Max(1, (selfPos - selfCollisionPos).magnitude);
avoidVector *= currentVelocity.magnitude / collisionDistance;
return(avoidVector);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
normal = normal.normalized;
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
return(true);
}
return(false);
}
public static TSRigidBody GetClosestLine(TSVector position)
{
TSRigidBody closestLine = Line.LineList[0];
FP closestResult = new FP(-1); // DOT product of -1, farthest possible value to check direction of a vector
foreach (TSRigidBody thisLine in Line.LineList)
{
FP result = TSVector.Dot(thisLine.tsTransform.forward, position.normalized);
if (result > closestResult)
{
closestResult = result;
closestLine = thisLine;
}
}
return(closestLine);
}
/// <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 maxDotProduct = FP.MinValue;
int maxIndex = 0;
FP dotProduct;
for (int i = 0, length = vertices.Count; i < length; i++)
{
dotProduct = TSVector.Dot(vertices[i], direction);
if (dotProduct > maxDotProduct)
{
maxDotProduct = dotProduct;
maxIndex = i;
}
}
result = vertices[maxIndex] - this.shifted;
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = TSVector.Dot(body1.linearVelocity, jacobian[0]);
jv += TSVector.Dot(body2.linearVelocity, jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
TSVector temp;
if (!body1.isStatic)
{
body1.ApplyImpulse(jacobian[0] * lambda);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(jacobian[1] * lambda);
}
}
public static FP LineCircleIntersectionFactor(TSVector circleCenter, TSVector linePoint1, TSVector linePoint2, FP radius)
{
FP segmentLength;
var normalizedDirection = Normalize(linePoint2 - linePoint1, out segmentLength);
var dirToStart = linePoint1 - circleCenter;
var dot = TSVector.Dot(dirToStart, normalizedDirection);
var discriminant = dot * dot - (dirToStart.sqrMagnitude - radius * radius);
if (discriminant < 0)
{
// No intersection, pick closest point on segment
discriminant = 0;
}
var t = -dot + TSMath.Sqrt(discriminant);
return(segmentLength > EPSILON ? t / segmentLength : 1);
}
private static int FindExtremePoint(List <TSVector> points, ref TSVector dir)
{
int index = 0;
FP current = FP.MinValue;
TSVector point; FP value;
for (int i = 1; i < points.Count; i++)
{
point = points[i];
value = TSVector.Dot(point, dir);
if (value > current)
{
current = value; index = i;
}
}
return(index);
}
/// <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)
{
p1 = realRelPos1 + body1.position;
}
else
{
p1 = body1.position + body1.Orientation.Multiply(realRelPos1);
}
if (body2IsMassPoint)
{
p2 = realRelPos2 + body2.position;
}
else
{
p2 = body2.position + body2.Orientation.Multiply(realRelPos2);
}
penetration = TSVector.Dot(p1 - p2, normal);
}
/// <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)
{
TSVector exp;
TSVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
FP min = TSVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
for (int i = 1, length = vecs.Length; i < length; i++)
{
FP dot = TSVector.Dot(ref vecs[i], ref direction);
if (dot > min)
{
min = dot;
minIndex = i;
}
}
result = vecs[minIndex] + exp;
}
public LimitHingeJoint3D(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>
/// 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);
}
}
/// <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)
{
TSVector expandVector;
TSVector.Normalize(ref direction, out expandVector);
TSVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(ref points[0], ref direction);
for (int i = 1, length = points.Length; i < length; i++)
{
FP dot = TSVector.Dot(ref points[i], ref direction);
if (dot > min)
{
min = dot;
minIndex = i;
}
}
TSVector.Add(ref points[minIndex], ref expandVector, out result);
}