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)); }