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 TSVector3 NormalizeCheck(TSVector3 v)
{
//防止溢出
FP div = TSMath.Max(TSMath.Abs(v.x), TSMath.Abs(v.y), TSMath.Abs(v.z));
return(v / div);
}
public static FP Distance(TSVector v1, TSVector v2)
{
FP result;
DistanceSquared(ref v1, ref v2, out result);
return(TSMath.Sqrt(result));
}
public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t)
{
t = TSMath.Clamp(t, 0, 1);
//t = 0.5f;
//if (t == 0)
// return from;
FP dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
if (dot >= FP.NearOne)
{
// If the inputs are too close for comfort, linearly interpolate
// and normalize the result.
TSQuaternion result = Lerp(from, to, t);
return(result);
}
FP halfTheta = FP.Acos(dot);
//UnityEngine.Debug.LogWarning("halfTheta:"+ halfTheta+" dot:"+dot+ " FP.Sin(halfTheta):"+ FP.Sin(halfTheta));
return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta)));
}
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 override void OnSyncedUpdate()
{
//if (useSpring)
//{
// //Adding a spring and damper Term to the Equation of Motion
// thisBody.AddTorque ((-1) * TSWorldAxis * ((thisJoint.getHingeAngle () - Spring.tagetPosition) * Spring.spring + thisJoint.getAngularVel () * Spring.damper));
//}
if (Input.GetKeyDown(KeyCode.T))
{
thisBody.AddTorque(new Vector3(0, 90, 0).ToTSVector());
}
Debug.Log(thisJoint.getHingeAngle() + " : " + thisJoint.getAngularVel());
//thisBody.tsTransform.rotation = TSQuaternion.Euler (TSMath.Clamp (thisBody.tsTransform.eulerAngles.x, -30, 30), thisBody.tsTransform.eulerAngles.y, thisBody.tsTransform.eulerAngles.z);
//CharacterJoint;
if (TSMath.Abs(thisJoint.AppliedImpulse) >= breakForce) //@TODO: Add break torque
{
thisJoint.Deactivate();
Destroy(this);
}
}
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 override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector max;
TSVector.Transform(ref this.halfSize, ref tSMatrix, out max);
box.max = max;
TSVector.Negate(ref max, out box.min);
}
public void Transform(ref TSMatrix orientation)
{
TSVector value = FP.Half * (this.max - this.min);
TSVector value2 = FP.Half * (this.max + this.min);
TSVector.Transform(ref value2, ref orientation, out value2);
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector.Transform(ref value, ref tSMatrix, out value);
this.max = value2 + value;
this.min = value2 - value;
}
public override int Prepare(ref TSBBox box)
{
bool flag = box.min.x < this.boundings.min.x;
if (flag)
{
this.minX = 0;
}
else
{
this.minX = (int)((long)FP.Floor((box.min.x - this.sphericalExpansion) / this.scaleX));
this.minX = (int)((long)TSMath.Max(this.minX, 0));
}
bool flag2 = box.max.x > this.boundings.max.x;
if (flag2)
{
this.maxX = this.heightsLength0 - 1;
}
else
{
this.maxX = (int)((long)FP.Ceiling((box.max.x + this.sphericalExpansion) / this.scaleX));
this.maxX = (int)((long)TSMath.Min(this.maxX, this.heightsLength0 - 1));
}
bool flag3 = box.min.z < this.boundings.min.z;
if (flag3)
{
this.minZ = 0;
}
else
{
this.minZ = (int)((long)FP.Floor((box.min.z - this.sphericalExpansion) / this.scaleZ));
this.minZ = (int)((long)TSMath.Max(this.minZ, 0));
}
bool flag4 = box.max.z > this.boundings.max.z;
if (flag4)
{
this.maxZ = this.heightsLength1 - 1;
}
else
{
this.maxZ = (int)((long)FP.Ceiling((box.max.z + this.sphericalExpansion) / this.scaleZ));
this.maxZ = (int)((long)TSMath.Min(this.maxZ, this.heightsLength1 - 1));
}
this.numX = this.maxX - this.minX;
this.numZ = this.maxZ - this.minZ;
return(this.numX * this.numZ * 2);
}
public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t)
{
t = TSMath.Clamp(t, 0, 1);
FP dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
FP halfTheta = FP.Acos(dot);
return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta)));
}
public override void Iterate()
{
bool flag = this.skipConstraint;
if (!flag)
{
FP x = TSVector.Dot(ref this.body1.linearVelocity, ref this.jacobian[0]);
x += TSVector.Dot(ref this.body2.linearVelocity, ref this.jacobian[1]);
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
bool flag2 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMinimumDistance;
if (flag2)
{
FP y2 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y2;
}
else
{
bool flag3 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMaximumDistance;
if (flag3)
{
FP y3 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y3;
}
else
{
this.accumulatedImpulse += fP;
}
}
bool flag4 = !this.body1.isStatic;
if (flag4)
{
TSVector tSVector;
TSVector.Multiply(ref this.jacobian[0], fP * this.body1.inverseMass, out tSVector);
TSVector.Add(ref tSVector, ref this.body1.linearVelocity, out this.body1.linearVelocity);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
TSVector tSVector;
TSVector.Multiply(ref this.jacobian[1], fP * this.body2.inverseMass, out tSVector);
TSVector.Add(ref tSVector, ref this.body2.linearVelocity, out this.body2.linearVelocity);
}
}
}
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;
this.normal.Normalize();
this.p1 = point1;
this.p2 = point2;
this.newContact = newContact;
TSVector.Subtract(ref this.p1, ref body1.position, out this.relativePos1);
TSVector.Subtract(ref this.p2, ref body2.position, out this.relativePos2);
TSVector.Transform(ref this.relativePos1, ref body1.invOrientation, out this.realRelPos1);
TSVector.Transform(ref this.relativePos2, ref body2.invOrientation, out this.realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
this.body1IsMassPoint = body1.isParticle;
this.body2IsMassPoint = body2.isParticle;
if (newContact)
{
this.treatBody1AsStatic = body1.isStatic;
this.treatBody2AsStatic = body2.isStatic;
this.accumulatedNormalImpulse = FP.Zero;
this.accumulatedTangentImpulse = FP.Zero;
this.lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
this.staticFriction = TSMath.Max(body1.material.staticFriction, body2.material.staticFriction);
this.dynamicFriction = TSMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
this.restitution = TSMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
this.staticFriction = TSMath.Min(body1.material.staticFriction, body2.material.staticFriction);
this.dynamicFriction = TSMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
this.restitution = TSMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
this.staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / (2 * FP.One);
this.dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / (2 * FP.One);
this.restitution = (body1.material.restitution + body2.material.restitution) / (2 * FP.One);
break;
}
}
this.settings = settings;
}
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
box.min = this.mInternalBBox.min;
box.max = this.mInternalBBox.max;
TSVector tSVector = FP.Half * (box.max - box.min);
TSVector tSVector2 = FP.Half * (box.max + box.min);
TSVector value;
TSVector.Transform(ref tSVector2, ref orientation, out value);
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector value2;
TSVector.Transform(ref tSVector, ref tSMatrix, out value2);
box.max = value + value2;
box.min = value - value2;
}
public override void Iterate()
{
bool flag = this.skipConstraint;
if (!flag)
{
FP x = this.body1.linearVelocity * this.jacobian[0] + this.body1.angularVelocity * this.jacobian[1] + this.body2.linearVelocity * this.jacobian[2] + this.body2.angularVelocity * this.jacobian[3];
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
bool flag2 = this.behavior == PointPointDistance.DistanceBehavior.LimitMinimumDistance;
if (flag2)
{
FP y2 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y2;
}
else
{
bool flag3 = this.behavior == PointPointDistance.DistanceBehavior.LimitMaximumDistance;
if (flag3)
{
FP y3 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y3;
}
else
{
this.accumulatedImpulse += fP;
}
}
bool flag4 = !this.body1.isStatic;
if (flag4)
{
this.body1.linearVelocity += this.body1.inverseMass * fP * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(fP * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
this.body2.linearVelocity += this.body2.inverseMass * fP * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(fP * this.jacobian[3], this.body2.invInertiaWorld);
}
}
}
public override void PrepareForIteration(FP timestep)
{
this.effectiveMass = this.body1.invInertiaWorld + this.body2.invInertiaWorld;
this.softnessOverDt = this.softness / timestep;
this.effectiveMass.M11 = this.effectiveMass.M11 + this.softnessOverDt;
this.effectiveMass.M22 = this.effectiveMass.M22 + this.softnessOverDt;
this.effectiveMass.M33 = this.effectiveMass.M33 + this.softnessOverDt;
TSMatrix.Inverse(ref this.effectiveMass, out this.effectiveMass);
TSMatrix value;
TSMatrix.Multiply(ref this.initialOrientation1, ref this.initialOrientation2, out value);
TSMatrix.Transpose(ref value, out value);
TSMatrix tSMatrix = value * this.body2.invOrientation * this.body1.orientation;
FP fP = tSMatrix.M32 - tSMatrix.M23;
FP fP2 = tSMatrix.M13 - tSMatrix.M31;
FP fP3 = tSMatrix.M21 - tSMatrix.M12;
FP fP4 = TSMath.Sqrt(fP * fP + fP2 * fP2 + fP3 * fP3);
FP x = tSMatrix.M11 + tSMatrix.M22 + tSMatrix.M33;
FP value2 = FP.Atan2(fP4, x - 1);
TSVector value3 = new TSVector(fP, fP2, fP3) * value2;
bool flag = fP4 != FP.Zero;
if (flag)
{
value3 *= FP.One / fP4;
}
this.bias = value3 * this.biasFactor * (-FP.One / timestep);
bool flag2 = !this.body1.IsStatic;
if (flag2)
{
this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse, this.body1.invInertiaWorld);
}
bool flag3 = !this.body2.IsStatic;
if (flag3)
{
this.body2.angularVelocity += TSVector.Transform(-FP.One * this.accumulatedImpulse, this.body2.invInertiaWorld);
}
}
internal void InverseTransform(ref TSVector position, ref TSMatrix orientation)
{
TSVector.Subtract(ref this.max, ref position, out this.max);
TSVector.Subtract(ref this.min, ref position, out this.min);
TSVector tSVector;
TSVector.Add(ref this.max, ref this.min, out tSVector);
tSVector.x *= FP.Half;
tSVector.y *= FP.Half;
tSVector.z *= FP.Half;
TSVector tSVector2;
TSVector.Subtract(ref this.max, ref this.min, out tSVector2);
tSVector2.x *= FP.Half;
tSVector2.y *= FP.Half;
tSVector2.z *= FP.Half;
TSVector.TransposedTransform(ref tSVector, ref orientation, out tSVector);
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector.TransposedTransform(ref tSVector2, ref tSMatrix, out tSVector2);
TSVector.Add(ref tSVector, ref tSVector2, out this.max);
TSVector.Subtract(ref tSVector, ref tSVector2, out this.min);
}
public static TSVector2 Min(TSVector2 value1, TSVector2 value2)
{
return(new TSVector2(
TSMath.Min(value1.x, value2.x),
TSMath.Min(value1.y, value2.y)));
}
public static TSVector2 CatmullRom(TSVector2 value1, TSVector2 value2, TSVector2 value3, TSVector2 value4, FP amount)
{
return(new TSVector2(
TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount)));
}
// Returns the angle in degrees between /from/ and /to/. This is always the smallest
public static FP Angle(TSVector from, TSVector to)
{
return(TSMath.Acos(TSMath.Clamp(Dot(from.normalized, to.normalized), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg);
}
public static TSVector2 Clamp(TSVector2 value1, TSVector2 min, TSVector2 max)
{
return(new TSVector2(
TSMath.Clamp(value1.x, min.x, max.x),
TSMath.Clamp(value1.y, min.y, max.y)));
}
public virtual void MakeHull(ref List <TSVector> triangleList, int generationThreshold)
{
FP zero = FP.Zero;
bool flag = generationThreshold < 0;
if (flag)
{
generationThreshold = 4;
}
Stack <Shape.ClipTriangle> stack = new Stack <Shape.ClipTriangle>();
TSVector[] array = new TSVector[]
{
new TSVector(-1, 0, 0),
new TSVector(1, 0, 0),
new TSVector(0, -1, 0),
new TSVector(0, 1, 0),
new TSVector(0, 0, -1),
new TSVector(0, 0, 1)
};
int[,] array2 = new int[, ]
{
{
5,
1,
3
},
{
4,
3,
1
},
{
3,
4,
0
},
{
0,
5,
3
},
{
5,
2,
1
},
{
4,
1,
2
},
{
2,
0,
4
},
{
0,
2,
5
}
};
for (int i = 0; i < 8; i++)
{
stack.Push(new Shape.ClipTriangle
{
n1 = array[array2[i, 0]],
n2 = array[array2[i, 1]],
n3 = array[array2[i, 2]],
generation = 0
});
}
while (stack.Count > 0)
{
Shape.ClipTriangle clipTriangle = stack.Pop();
TSVector tSVector;
this.SupportMapping(ref clipTriangle.n1, out tSVector);
TSVector tSVector2;
this.SupportMapping(ref clipTriangle.n2, out tSVector2);
TSVector tSVector3;
this.SupportMapping(ref clipTriangle.n3, out tSVector3);
FP sqrMagnitude = (tSVector2 - tSVector).sqrMagnitude;
FP sqrMagnitude2 = (tSVector3 - tSVector2).sqrMagnitude;
FP sqrMagnitude3 = (tSVector - tSVector3).sqrMagnitude;
bool flag2 = TSMath.Max(TSMath.Max(sqrMagnitude, sqrMagnitude2), sqrMagnitude3) > zero && clipTriangle.generation < generationThreshold;
if (flag2)
{
Shape.ClipTriangle item = default(Shape.ClipTriangle);
Shape.ClipTriangle item2 = default(Shape.ClipTriangle);
Shape.ClipTriangle item3 = default(Shape.ClipTriangle);
Shape.ClipTriangle item4 = default(Shape.ClipTriangle);
item.generation = clipTriangle.generation + 1;
item2.generation = clipTriangle.generation + 1;
item3.generation = clipTriangle.generation + 1;
item4.generation = clipTriangle.generation + 1;
item.n1 = clipTriangle.n1;
item2.n2 = clipTriangle.n2;
item3.n3 = clipTriangle.n3;
TSVector tSVector4 = FP.Half * (clipTriangle.n1 + clipTriangle.n2);
tSVector4.Normalize();
item.n2 = tSVector4;
item2.n1 = tSVector4;
item4.n3 = tSVector4;
tSVector4 = FP.Half * (clipTriangle.n2 + clipTriangle.n3);
tSVector4.Normalize();
item2.n3 = tSVector4;
item3.n2 = tSVector4;
item4.n1 = tSVector4;
tSVector4 = FP.Half * (clipTriangle.n3 + clipTriangle.n1);
tSVector4.Normalize();
item.n3 = tSVector4;
item3.n1 = tSVector4;
item4.n2 = tSVector4;
stack.Push(item);
stack.Push(item2);
stack.Push(item3);
stack.Push(item4);
}
else
{
bool flag3 = ((tSVector3 - tSVector) % (tSVector2 - tSVector)).sqrMagnitude > TSMath.Epsilon;
if (flag3)
{
triangleList.Add(tSVector);
triangleList.Add(tSVector2);
triangleList.Add(tSVector3);
}
}
}
}
public static void SmoothStep(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result)
{
result = new TSVector2(
TSMath.SmoothStep(value1.x, value2.x, amount),
TSMath.SmoothStep(value1.y, value2.y, amount));
}
public static TSVector2 SmoothStep(TSVector2 value1, TSVector2 value2, FP amount)
{
return(new TSVector2(
TSMath.SmoothStep(value1.x, value2.x, amount),
TSMath.SmoothStep(value1.y, value2.y, amount)));
}
public static void Min(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
{
result.x = TSMath.Min(value1.x, value2.x);
result.y = TSMath.Min(value1.y, value2.y);
}
public static void Clamp(ref TSVector2 value1, ref TSVector2 min, ref TSVector2 max, out TSVector2 result)
{
result = new TSVector2(
TSMath.Clamp(value1.x, min.x, max.x),
TSMath.Clamp(value1.y, min.y, max.y));
}
public static void LerpUnclamped(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result)
{
result = new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount));
}
public static TSVector2 LerpUnclamped(TSVector2 value1, TSVector2 value2, FP amount)
{
return(new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount)));
}
public static void Hermite(ref TSVector2 value1, ref TSVector2 tangent1, ref TSVector2 value2, ref TSVector2 tangent2,
FP amount, out TSVector2 result)
{
result.x = TSMath.Hermite(value1.x, tangent1.x, value2.x, tangent2.x, amount);
result.y = TSMath.Hermite(value1.y, tangent1.y, value2.y, tangent2.y, amount);
}
