/// <summary>
/// Creates a matrix for rotating points around the Y-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateY(FP radians, TSVector centerPoint)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
FP x = centerPoint.x * (FP.One - c) - centerPoint.z * s;
FP z = centerPoint.x * (FP.One - c) + centerPoint.x * s;
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ x 0 z 1 ]
result.M11 = c;
result.M12 = FP.Zero;
result.M13 = -s;
result.M14 = FP.Zero;
result.M21 = FP.Zero;
result.M22 = FP.One;
result.M23 = FP.Zero;
result.M24 = FP.Zero;
result.M31 = s;
result.M32 = FP.Zero;
result.M33 = c;
result.M34 = FP.Zero;
result.M41 = x;
result.M42 = FP.Zero;
result.M43 = z;
result.M44 = FP.One;
return(result);
}
internal static FP Pow2(FP x)
{
if (x.RawValue == 0)
{
return(FP.One);
}
// Avoid negative arguments by exploiting that exp(-x) = 1/exp(x).
bool neg = x.RawValue < 0;
if (neg)
{
x = -x;
}
// static readonly FP Log2Max = new FP(LOG2MAX);
// static readonly FP Log2Min = new FP(LOG2MIN);
if (x == FP.One)
{
return(neg ? FP.Half : 2);
}
if (x >= LOG2MAX)
{
return(neg ? FP.One / FP.MaxValue : FP.MaxValue);
}
if (x <= LOG2MIN)
{
return(neg ? FP.MaxValue : FP.Zero);
}
/* The algorithm is based on the power series for exp(x):
* http://en.wikipedia.org/wiki/Exponential_function#Formal_definition
*
* From term n, we get term n+1 by multiplying with x/n.
* When the sum term drops to zero, we can stop summing.
*/
int integerPart = TSMath.Floor(x).AsInt();
// Take fractional part of exponent
x._serializedValue = x._serializedValue & 0x00000000FFFFFFFF;
var result = FP.One;
var term = FP.One;
int i = 1;
while (term._serializedValue != 0)
{
term = FastMul(FastMul(x, term), LN2) / i;
result += term;
i++;
}
result._serializedValue = (result._serializedValue << integerPart);
if (neg)
{
result = FP.One / result;
}
return(result);
}
/// <summary>
/// Creates a matrix for rotating points around the Z-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateZ(FP radians, TSVector centerPoint)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
FP x = centerPoint.x * (1 - c) + centerPoint.y * s;
FP y = centerPoint.y * (1 - c) - centerPoint.x * s;
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ x y 0 1 ]
result.M11 = c;
result.M12 = s;
result.M13 = FP.Zero;
result.M14 = FP.Zero;
result.M21 = -s;
result.M22 = c;
result.M23 = FP.Zero;
result.M24 = FP.Zero;
result.M31 = FP.Zero;
result.M32 = FP.Zero;
result.M33 = FP.One;
result.M34 = FP.Zero;
result.M41 = x;
result.M42 = y;
result.M43 = FP.Zero;
result.M44 = FP.One;
return(result);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
TSVector2 value = tSVector + MathUtils.Cross(fP, this._rA);
TSVector2 value2 = tSVector2 + MathUtils.Cross(fP2, this._rB);
FP fP3 = this._length - this.MaxLength;
FP fP4 = TSVector2.Dot(this._u, value2 - value);
bool flag = fP3 < 0f;
if (flag)
{
fP4 += data.step.inv_dt * fP3;
}
FP fP5 = -this._mass * fP4;
FP impulse = this._impulse;
this._impulse = TSMath.Min(0f, this._impulse + fP5);
fP5 = this._impulse - impulse;
TSVector2 tSVector3 = fP5 * this._u;
tSVector -= this._invMassA * tSVector3;
fP -= this._invIA * MathUtils.Cross(this._rA, tSVector3);
tSVector2 += this._invMassB * tSVector3;
fP2 += this._invIB * MathUtils.Cross(this._rB, tSVector3);
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}
public static TSVector Clamp(TSVector val, TSVector min, TSVector max)
{
TSVector vec = TSVector.zero;
vec.Set(TSMath.Clamp(val.x, min.x, max.x), TSMath.Clamp(val.y, min.y, max.y), TSMath.Clamp(val.z, min.z, max.z));
return(vec);
}
public static void CatmullRom(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, ref TSVector2 value4,
FP amount, out TSVector2 result)
{
result = new TSVector2(
TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
}
public static TSVector2?FindIntersection(LineSegment a, LineSegment b)
{
FP x1 = a.A.Position.x;
FP y1 = a.A.Position.y;
FP x2 = a.B.Position.x;
FP y2 = a.B.Position.y;
FP x3 = b.A.Position.x;
FP y3 = b.A.Position.y;
FP x4 = b.B.Position.x;
FP y4 = b.B.Position.y;
FP denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
FP uaNum = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
FP ubNum = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
FP ua = uaNum / denom;
FP ub = ubNum / denom;
if (TSMath.Clamp(ua, 0f, 1f) != ua || TSMath.Clamp(ub, 0f, 1f) != ub)
{
return(null);
}
return(a.A.Position + (a.B.Position - a.A.Position) * ua);
}
public FP ComputeSquaredDistanceToPoint(TSVector Point)
{
//
FP DistSquared = FP.Zero;
if (Point.x < min.x)
{
DistSquared += TSMath.Sqrt(Point.x - min.x);
}
else if (Point.x > max.x)
{
DistSquared += TSMath.Sqrt(Point.x - max.x);
}
if (Point.z < min.z)
{
DistSquared += TSMath.Sqrt(Point.z - min.z);
}
else if (Point.z > max.z)
{
DistSquared += TSMath.Sqrt(Point.z - max.z);
}
return(DistSquared);
}
/// <summary>
/// Creates a matrix for rotating points around the Y-axis.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateY(FP radians)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M12 = FP.Zero;
result.M13 = -s;
result.M14 = FP.Zero;
result.M21 = FP.Zero;
result.M22 = FP.One;
result.M23 = FP.Zero;
result.M24 = FP.Zero;
result.M31 = s;
result.M32 = FP.Zero;
result.M33 = c;
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = FP.Zero;
result.M43 = FP.Zero;
result.M44 = FP.One;
return(result);
}
/** Bias towards the right side of agents.
* Rotate desiredVelocity at most [value] number of radians. 1 radian ≈ 57°
* This breaks up symmetries.
*
* The desired velocity will only be rotated if it is inside a velocity obstacle (VO).
* If it is inside one, it will not be rotated further than to the edge of it
*
* The targetPointInVelocitySpace will be rotated by the same amount as the desired velocity
*
* \returns True if the desired velocity was inside any VO
*/
static bool BiasDesiredVelocity(VOBuffer vos, ref TSVector2 desiredVelocity, ref TSVector2 targetPointInVelocitySpace, FP maxBiasRadians)
{
var desiredVelocityMagn = desiredVelocity.magnitude;
FP maxValue = FP.Zero;
for (int i = 0; i < vos.length; i++)
{
FP value;
// The value is approximately the distance to the edge of the VO
// so taking the maximum will give us the distance to the edge of the VO
// which the desired velocity is furthest inside
vos.buffer[i].Gradient(desiredVelocity, out value);
maxValue = TSMath.Max(maxValue, value);
}
// Check if the agent was inside any VO
var inside = maxValue > 0;
// Avoid division by zero below
if (desiredVelocityMagn < FP.One / 1000)
{
return(inside);
}
// Rotate the desired velocity clockwise (to the right) at most maxBiasRadians number of radians
// Assuming maxBiasRadians is small, we can just move it instead and it will give approximately the same effect
// See https://en.wikipedia.org/wiki/Small-angle_approximation
var angle = TSMath.Min(maxBiasRadians, maxValue / desiredVelocityMagn);
desiredVelocity += new TSVector2(desiredVelocity.y, -desiredVelocity.x) * angle;
targetPointInVelocitySpace += new TSVector2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * angle;
return(inside);
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
TSVector2 tSVector = MathUtils.Mul(ref xf, this.Position);
FP fP = -(TSVector2.Dot(normal, tSVector) - offset);
bool flag = fP < -base.Radius + Settings.Epsilon;
FP result;
if (flag)
{
result = 0;
}
else
{
bool flag2 = fP > base.Radius;
if (flag2)
{
sc = tSVector;
result = Settings.Pi * this._2radius;
}
else
{
FP y = fP * fP;
FP fP2 = this._2radius * (TSMath.Asin(fP / base.Radius) + TSMath.PiOver2 + fP * TSMath.Sqrt(this._2radius - y));
FP y2 = new FP(-2) / new FP(3) * Math.Pow((double)(this._2radius - y).AsFloat(), 1.5) / fP2;
sc.x = tSVector.x + normal.x * y2;
sc.y = tSVector.y + normal.y * y2;
result = fP2;
}
}
return(result);
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
TSVector2 p = MathUtils.Mul(ref xf, Position);
FP l = -(TSVector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * _2radius);
}
//Magic
FP l2 = l * l;
FP area = _2radius * (FP)((TSMath.Asin((l / Radius)) + TSMath.PiOver2) + l * TSMath.Sqrt(_2radius - l2));
// TODO - PORT
//FP com = -2.0f / 3.0f * (FP)Math.Pow(_2radius - l2, 1.5f) / area;
FP com = new FP(-2) / new FP(3) * (FP)Math.Pow((_2radius - l2).AsFloat(), 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return(area);
}
internal static FP CalculateHValue(IInt2 gridPos1, IInt2 gridPos2, Heuristic heuristic)
{
FP h = FP.Zero;
switch (heuristic)
{
case Heuristic.Euclidean:
h = TSMath.Sqrt((gridPos1 - gridPos2).sqrMagnitudeLong) * HeuristicFactor;
break;
case Heuristic.Manhattan:
h = (Math.Abs(gridPos2.x - gridPos1.x) + Math.Abs(gridPos2.y - gridPos1.y)) * HeuristicFactor;
// return h;
break;
case Heuristic.DiagonalManhattan:
IInt2 p = gridPos2 - gridPos1;
p.x = Math.Abs(p.x);
p.y = Math.Abs(p.y);
int diag = Math.Min(p.x, p.y);
int diag2 = Math.Max(p.x, p.y);
h = ((CustomMath.DiagonalCost * diag + (diag2 - diag))) * HeuristicFactor;
break;
}
return(h * GridMap.GetNodeSize());
}
/// <summary>
// determins if the shot hit the target.
/// <summary>
bool ishit(_Ship inputship)
{
bool output = false;
if (guntypemain == guntype.Missile && inputship.HullType != eHullType.Light)
{
output = true;
}
else
{
FP chance = TSMath.Round(100 * getmodifyer(inputship.HullType));
chanceout = chance;
if (parentscript.isTrackingimproved)
{
chance = chance * 2;
}
FP randomnum = customran.Next(0, 100);
if (chance > randomnum)
{
output = true;
}
else
{
output = false;
}
teststring = chance.ToString() + " is more? than " + randomnum.ToString() + " so it is a hit: " + output;
}
return(output);
}
internal void Extend(Envelope by)
{
X1 = TSMath.Min(X1, by.X1);
Y1 = TSMath.Min(Y1, by.Y1);
X2 = TSMath.Max(X2, by.X2);
Y2 = TSMath.Max(Y2, by.Y2);
}
/// <summary>
/// Creates a matrix for rotating points around the X-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateX(FP radians, TSVector centerPoint)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
FP y = centerPoint.y * (FP.One - c) + centerPoint.z * s;
FP z = centerPoint.z * (FP.One - c) - centerPoint.y * s;
// [ 1 0 0 0 ]
// [ 0 c s 0 ]
// [ 0 -s c 0 ]
// [ 0 y z 1 ]
result.M11 = FP.One;
result.M12 = FP.Zero;
result.M13 = FP.Zero;
result.M14 = FP.Zero;
result.M21 = FP.Zero;
result.M22 = c;
result.M23 = s;
result.M24 = FP.Zero;
result.M31 = FP.Zero;
result.M32 = -s;
result.M33 = c;
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = y;
result.M43 = z;
result.M44 = FP.One;
return(result);
}
public static void Barycentric(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, FP amount1,
FP amount2, out TSVector2 result)
{
result = new TSVector2(
TSMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
TSMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
}
public static RectInt ToRect(this SDFRawData sdf, TSVector2 start, TSVector2 end)
{
TSVector2 min = new TSVector2(TSMath.Min(start.x, end.x), TSMath.Min(start.y, end.y));
TSVector2 max = new TSVector2(TSMath.Max(start.x, end.x), TSMath.Max(start.y, end.y));
return(new RectInt(sdf.FloorToGrid(min), sdf.CeilingToGrid(max)));
}
private TSVector TargetPosition(int index, TSVector sphere, int agentsnum)
{
if (agentsnum != 0)
{
int separation = 150;
agentsPerSide[index] = agentsnum / 3 + (agentsnum % 3 > 0 ? 1 : 0);
int length = agentsnum * 200;
int side = index % 3;
FP lengthMultiplier = (index / 3) / (FP)agentsPerSide[side];
lengthMultiplier = 1 - (lengthMultiplier - (int)lengthMultiplier);
FP height = length / 2 * TSMath.Sqrt(3); // Equilaterial triangle height
if (index == 0)
{
return(sphere);
}
else
{
return(sphere + new TSVector(separation * (index % 2 == 0 ? -1 : 1) * (((index - 1) / 2) + 1), 0, separation * (((index - 1) / 2) + 1)));
}
}
else
{
return(sphere);
}
}
public static TSVector2 Lerp(TSVector2 value1, TSVector2 value2, FP amount)
{
amount = TSMath.Clamp(amount, 0, 1);
return(new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount)));
}
// The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees.
// If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper.
// The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction.
public static FP SignedAngle(TSVector from, TSVector to, TSVector axis)
{
TSVector fromNorm = from.normalized, toNorm = to.normalized;
FP unsignedAngle = TSMath.Acos(TSMath.Clamp(Dot(fromNorm, toNorm), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg;
FP sign = TSMath.Sign(Dot(axis, Cross(fromNorm, toNorm)));
return(unsignedAngle * sign);
}
public static TSVector2 Move(this SDFMap map, List <DynamicCircle> circles, FP radius, TSVector2 start, TSVector2 dir, FP len, int layerMask = -1)
{
start = map.SDF.WorldToLocal(start);
TSVector2 end = start + dir * len;
RectInt rect = map.SDF.ToRect(start, end);
int externSize = (int)TSMath.Ceiling(radius / map.SDF.Grain);
rect.max += new Vector2Int(externSize, externSize);
moveFilterCache.Clear();
map.FilterToList(moveFilterCache, rect, layerMask);
FP maxStepLen = radius * FP.Half;
int moveStep = (int)TSMath.Ceiling(len / maxStepLen);
TSVector2 result = start;
for (int i = 1; i <= moveStep; ++i)
{
FP moveLen = maxStepLen;
if (i == moveStep)
{
moveLen = len - (moveStep - 1) * maxStepLen;
}
TSVector2 newPos = result + dir * moveLen;
FP sd = map.Sample(newPos, moveFilterCache, circles);
if (sd < radius)
{
TSVector2 gradient = map.Gradient(moveFilterCache, circles, newPos);
TSVector2 asjustDir = dir - gradient * TSVector2.Dot(gradient, dir);
newPos = result + asjustDir.normalized * moveLen;
//多次迭代
for (int j = 0; j < 3; ++j)
{
sd = map.Sample(newPos, moveFilterCache, circles);
if (sd >= radius)
{
break;
}
newPos += map.Gradient(moveFilterCache, circles, newPos) * (radius - sd);
}
//避免往返
if (TSVector2.Dot(newPos - start, dir) < FP.Zero)
{
return(result + map.SDF.Origin);
}
else
{
result = newPos;
}
break;
}
else
{
result = newPos;
}
}
return(result + map.SDF.Origin);
}
private static FP CombinedArea(Envelope what, Envelope with)
{
var minX1 = TSMath.Max(what.X1, with.X1);
var minY1 = TSMath.Max(what.Y1, with.Y1);
var maxX2 = TSMath.Min(what.X2, with.X2);
var maxY2 = TSMath.Min(what.Y2, with.Y2);
return((maxX2 - minX1) * (maxY2 - minY1));
}
private static FP IntersectionArea(Envelope what, Envelope with)
{
var minX = TSMath.Max(what.X1, with.X1);
var minY = TSMath.Max(what.Y1, with.Y1);
var maxX = TSMath.Min(what.X2, with.X2);
var maxY = TSMath.Min(what.Y2, with.Y2);
return(TSMath.Max(0, maxX - minX) * TSMath.Max(0, maxY - minY));
}
/** \copydoc Pathfinding::RVO::IAgent::SetTarget */
public void SetTarget(TSVector2 targetPoint, FP desiredSpeed, FP maxSpeed)
{
maxSpeed = TSMath.Max(maxSpeed, 0);
desiredSpeed = TSMath.Min(TSMath.Max(desiredSpeed, 0), maxSpeed);
nextTargetPoint = targetPoint;
nextDesiredSpeed = desiredSpeed;
nextMaxSpeed = maxSpeed;
}
/// <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;
CBFrame.Utils.Logger.Debug("line812 body2.isStatic:" + body2.isStatic + ",body1.isStatic:" + body1.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;
}
//不旋转的box
public static FP SDBox(TSVector2 x, TSVector2 c, TSVector2 b)
{
TSVector2 p = x - c;
p.x = TSMath.Abs(p.x);
p.y = TSMath.Abs(p.y);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
//旋转的box
public static FP SDOrientedBox(TSVector2 x, TSVector2 c, TSVector2 rot, TSVector2 b)
{
TSVector2 v = x - c;
FP px = TSMath.Abs(TSVector2.Dot(v, rot)); //在box的x轴的投影长度
FP py = TSMath.Abs(TSVector2.Dot(v, new TSVector2(-rot.y, rot.x))); //在box的y轴的投影长度
TSVector2 p = new TSVector2(px, py);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
TSMatrix abs; TSMath.Absolute(ref orientation, out abs);
TSVector temp;
TSVector.Transform(ref halfSize, ref abs, out temp);
box.max = temp;
TSVector.Negate(ref temp, out box.min);
}
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);
}
