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 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);
}
// 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 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)));
}
private FP GetPercent(FP distance, FP radius)
{
//(1-(distance/radius))^power-1
// TODO - PORT
// FP percent = (FP)Math.Pow(1 - ((distance - radius) / radius), Power) - 1;
FP percent = (FP)Math.Pow((1 - ((distance - radius) / radius)).AsFloat(), Power.AsFloat()) - 1;
if (FP.IsNaN(percent))
{
return(0f);
}
return(TSMath.Clamp(percent, 0f, 1f));
}
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 Node GetNodeFromPoint(FP nodeX, FP nodeY)
{
FP percentX = nodeX / (FP)_nodeAmountX;
FP percentY = nodeY / (FP)_nodeAmountY;
percentX = TSMath.Clamp(percentX, FP.Zero, FP.One);
percentY = TSMath.Clamp(percentY, FP.Zero, FP.One);
int x = (int)TSMath.Round((((_nodeAmountX / _nodeUnitSize)) * percentX));
int y = (int)TSMath.Round((((_nodeAmountY / _nodeUnitSize)) * percentY));
x = Mathf.Clamp(x, 0, _nodeAmountX - 1);
y = Mathf.Clamp(y, 0, _nodeAmountY - 1);
return(_grid[x, y]);
}
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)));
}
/// <summary>
/// 移动
/// </summary>
private void Move()
{
if (!(_movX == 0 && _movY == 0))
{
bool gotIt = true;
}
tsRigidBody.velocity = new TSVector(_movX * speed, 0, _movY * speed);
// 限制角色移动范围
tsRigidBody.position = new TSVector(
TSMath.Clamp(tsRigidBody.position.x, boundary.xMin, boundary.xMax),
0,
TSMath.Clamp(tsRigidBody.position.z, boundary.zMin, boundary.zMax)
);
// 左右平移时稍微倾斜一下机身（绕 z 轴）
tsRigidBody.rotation = TSQuaternion.Euler(0, 0, tsRigidBody.velocity.x * -tilt);
// 旋转方向
if (_rotX == 0 && _rotY == 0)
{
tsTransform.rotation = TSQuaternion.Lerp(tsTransform.rotation, TSQuaternion.identity,
TrueSyncManager.DeltaTime * rotateSpeed);
}
else
{
TSVector joystickKnobPos = new TSVector(_rotX, 0, _rotY);
TSQuaternion targetRot = TSQuaternion.LookRotation(joystickKnobPos);
tsTransform.rotation =
TSQuaternion.Lerp(tsTransform.rotation, targetRot, TrueSyncManager.DeltaTime * rotateSpeed);
}
// 旋转粒子系统
float r = (float)tsTransform.rotation.eulerAngles.y * Mathf.Deg2Rad;
if (null != particleSystem1)
{
particleSystem1.startRotation = r;
}
if (null != particleSystem2)
{
particleSystem2.startRotation = r;
}
}
public override void ApplyForce(FP dt, FP strength)
{
foreach (Body body in World.BodyList)
{
//TODO: Consider Force Type
FP decayMultiplier = GetDecayMultiplier(body);
if (decayMultiplier != 0)
{
TSVector2 forceVector;
if (ForceType == ForceTypes.Point)
{
forceVector = body.Position - Position;
}
else
{
Direction.Normalize();
forceVector = Direction;
if (forceVector.magnitude == 0)
{
forceVector = new TSVector2(0, 1);
}
}
//TODO: Consider Divergence:
//forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (FP)Randomize.NextFP()));
// Calculate random Variation
if (Variation != 0)
{
FP strengthVariation = TrueSync.TSRandom.value * TSMath.Clamp(Variation, 0, 1);
forceVector.Normalize();
body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation);
}
else
{
forceVector.Normalize();
body.ApplyForce(forceVector * strength * decayMultiplier);
}
}
}
}
public void integrate(FP duration)
{
if (inverseMass <= 0)
{
return;
}
TSVector movement = this.velocity * duration;
this.position += movement;
TSVector resultingAcc = acceleration;
resultingAcc += this.forceAccum * this.inverseMass;
this.velocity *= TSMath.Clamp(FP.ONE - duration * this.damping, 0.0f, 1.0f);
this.clearAccumulator();
}
/// <summary>
/// Creates a gear shape with the specified radius and number of teeth.
/// </summary>
/// <param name="radius">The radius.</param>
/// <param name="numberOfTeeth">The number of teeth.</param>
/// <param name="tipPercentage">The tip percentage.</param>
/// <param name="toothHeight">Height of the tooth.</param>
/// <returns></returns>
public static Vertices CreateGear(FP radius, int numberOfTeeth, FP tipPercentage, FP toothHeight)
{
Vertices vertices = new Vertices();
FP stepSize = FP.PiTimes2 / numberOfTeeth;
tipPercentage /= 100f;
TSMath.Clamp(tipPercentage, 0f, 1f);
FP toothTipStepSize = (stepSize / 2f) * tipPercentage;
FP toothAngleStepSize = (stepSize - (toothTipStepSize * 2f)) / 2f;
for (int i = numberOfTeeth - 1; i >= 0; --i)
{
if (toothTipStepSize > 0f)
{
vertices.Add(
new TSVector2(radius *
FP.Cos(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize),
-radius *
FP.Sin(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize)));
vertices.Add(
new TSVector2((radius + toothHeight) *
FP.Cos(stepSize * i + toothAngleStepSize + toothTipStepSize),
-(radius + toothHeight) *
FP.Sin(stepSize * i + toothAngleStepSize + toothTipStepSize)));
}
vertices.Add(new TSVector2((radius + toothHeight) *
FP.Cos(stepSize * i + toothAngleStepSize),
-(radius + toothHeight) *
FP.Sin(stepSize * i + toothAngleStepSize)));
vertices.Add(new TSVector2(radius * FP.Cos(stepSize * i),
-radius * FP.Sin(stepSize * i)));
}
return(vertices);
}
public void FixedUpdate()
{
this.wheelAngle = TSMath.Lerp(this.wheelAngle, this.steerAngle, Time.fixedDeltaTime * this.steerTime);
if (TSPhysics.Raycast(transform.position.ToTSVector(), (-transform.up).ToTSVector(), out TSRaycastHit hit, maxLength + wheelRadius))
{
lastLength = springLength;
springLength = hit.distance.AsFloat() - wheelRadius;
springLength = TSMath.Clamp(springLength, minLength, maxLength).AsFloat();
springVelocity = (lastLength - springLength) / Time.fixedDeltaTime;
springForce = springStiffness * (restLength - springLength);
damperForce = damperStiffness * springVelocity;
suspensionForce = (springForce + damperForce) * transform.up.ToTSVector();
this.wheelVelocityLS = this.tsTransform.InverseTransformDirection(this.trb.GetPointVelocity(hit.point));
this.Fx = Input.GetAxis("Vertical") * springForce * speed;
this.Fy = wheelVelocityLS.x * springForce;
this.trb.AddForceAtPosition(suspensionForce + (Fx * transform.forward.ToTSVector()) + (Fy * (transform.right.ToTSVector() * -1)), hit.point);
}
}
/// <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>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
TSVector dv = CalculateRelativeVelocity();
FP kNormal = FP.Zero;
TSVector rantra = TSVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
TSVector.Cross(ref relativePos1, ref normal, out rantra);
TSVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
TSVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
TSVector rbntrb = TSVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
TSVector.Cross(ref relativePos2, ref normal, out rbntrb);
TSVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
TSVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kNormal += TSVector.Dot(ref rantra, ref normal);
}
if (!treatBody2AsStatic)
{
kNormal += TSVector.Dot(ref rbntrb, ref normal);
}
massNormal = FP.One / kNormal;
tangent = dv - TSVector.Dot(dv, normal) * normal;
tangent.Normalize();
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
TSVector.Cross(ref relativePos1, ref normal, out rantra);
TSVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
TSVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
TSVector.Cross(ref relativePos2, ref tangent, out rbntrb);
TSVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
TSVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
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 = TSVector.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 = -TSVector.Dot(ref tangent, ref dv);
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.
//同时求解和恢复是不可能的，所以当有新的接触时，仅仅应用恢复脉冲，有点虚假。
if (relNormalVel < -FP.One && newContact)
{
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.
// 如果穿透是负的（这意味着物体还没有接触，但是它们将来会接触），
// 我们将当前弹跳偏置存储在变量“lostSpeculativeBounce”中，并在下一个框架中应用它，此时投机接触已经解决。
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(ref impulse);
lastTimeStep = timestep;
newContact = false;
}
/// <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)
{
var dv = CalculateRelativeVelocity();
var kn = CalcK(body1, relativePos1, normal) +
CalcK(body2, relativePos2, normal);
massNormal = FP.One / kn;
tangent = dv - TSVector.Dot(dv, normal) * normal;
tangent.Normalize();
var kt = CalcK(body1, relativePos1, tangent) +
CalcK(body2, relativePos2, tangent);
massTangent = FP.One / kt;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = TSVector.Dot(normal, 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 = -TSVector.Dot(tangent, dv);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -FP.One && newContact)
{
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;
}
var impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(impulse);
lastTimeStep = timestep;
newContact = false;
}
/// <summary>
/// Gets the closest two points on two line segments.
/// </summary>
/// <remarks>
/// If the line segments are parallel and overlap in their common direction, then the midpoint of the overlapped portion of line segments
/// is returned.
/// </remarks>
/// <param name="sa">The first line segment.</param>
/// <param name="sb">The second line segment.</param>
/// <param name="scalarA">Returns a value between 0 and 1 indicating the position of the closest point on the first segment.</param>
/// <param name="pa">Returns the closest point on the first segment.</param>
/// <param name="scalarB">Returns a value between 0 and 1 indicating the position of the closest point on the second segment.</param>
/// <param name="pb">Returns the closest point on the second segment.</param>
public static void ClosestPoints(ref SegmentShape sa, ref SegmentShape sb,
out FP scalarA, out TSVector pa, out FP scalarB, out TSVector pb)
{
TSVector d1, d2, r;
TSVector.Subtract(ref sa.P2, ref sa.P1, out d1);
TSVector.Subtract(ref sb.P2, ref sb.P1, out d2);
TSVector.Subtract(ref sa.P1, ref sb.P1, out r);
FP a, e, f;
a = TSVector.Dot(ref d1, ref d1);
e = TSVector.Dot(ref d2, ref d2);
f = TSVector.Dot(ref d2, ref r);
if (a < TSMath.Epsilon && e < TSMath.Epsilon)
{
// segment a and b are both points
scalarA = scalarB = FP.Zero;
pa = sa.P1;
pb = sb.P1;
return;
}
if (a < TSMath.Epsilon)
{
// segment a is a point
scalarA = FP.Zero;
scalarB = TSMath.Clamp(f / e, FP.Zero, FP.One);
}
else
{
FP c = TSVector.Dot(ref d1, ref r);
if (e < TSMath.Epsilon)
{
// segment b is a point
scalarB = FP.Zero;
scalarA = TSMath.Clamp(-c / a, FP.Zero, FP.One);
}
else
{
FP b = TSVector.Dot(ref d1, ref d2);
FP denom = a * e - b * b;
if (denom < TSMath.Epsilon)
{
// segments are parallel
FP a1, a2, b1, b2;
a1 = TSVector.Dot(ref d2, ref sa.P1);
a2 = TSVector.Dot(ref d2, ref sa.P2);
b1 = TSVector.Dot(ref d2, ref sb.P1);
b2 = TSVector.Dot(ref d2, ref sb.P2);
if (a1 <= b1 && a2 <= b1)
{
// segment A is completely "before" segment B
scalarA = a2 > a1 ? FP.One : FP.Zero;
scalarB = FP.Zero;
}
else if (a1 >= b2 && a2 >= b2)
{
// segment B is completely "before" segment A
scalarA = a2 > a1 ? FP.Zero : FP.One;
scalarB = FP.One;
}
else
{
// segments A and B overlap, use midpoint of shared length
if (a1 > a2)
{
f = a1; a1 = a2; a2 = f;
}
f = (TSMath.Min(a2, b2) + TSMath.Max(a1, b1)) / 2;
scalarB = (f - b1) / e;
TSVector.Multiply(ref d2, scalarB, out pb);
TSVector.Add(ref sb.P1, ref pb, out pb);
sa.ClosestPointTo(ref pb, out scalarA, out pa);
return;
}
}
else
{
// general case
scalarA = TSMath.Clamp((b * f - c * e) / denom, FP.Zero, FP.One);
scalarB = (b * scalarA + f) / e;
if (scalarB < FP.Zero)
{
scalarB = FP.Zero;
scalarA = TSMath.Clamp(-c / a, FP.Zero, FP.One);
}
else if (scalarB > FP.One)
{
scalarB = FP.One;
scalarA = TSMath.Clamp((b - c) / a, FP.Zero, FP.One);
}
}
}
}
TSVector.Multiply(ref d1, scalarA, out d1);
TSVector.Multiply(ref d2, scalarB, out d2);
TSVector.Add(ref sa.P1, ref d1, out pa);
TSVector.Add(ref sb.P1, ref d2, out pb);
}
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 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)));
}
// 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 TSQuaternion Lerp(TSQuaternion a, TSQuaternion b, FP t)
{
t = TSMath.Clamp(t, FP.Zero, FP.One);
return(LerpUnclamped(a, b, t));
}