vlad.os/LuaCsForBarotraumaEP
0
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

(61d00a474) v0.9.7.1

Browse Source
This commit is contained in:
Regalis
2020-03-04 13:04:10 +01:00
parent 3c50efa5c9
commit 3c09ebe02f
5086 changed files with 786063 additions and 295871 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs
+26 -18
View File
@@ -1,3 +1,8 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
@@ -23,6 +28,7 @@
using System;
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -187,10 +193,11 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
_rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
_rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
_u = cB + _rB - cA - _rA;
// Handle singularity.
@@ -204,8 +211,8 @@ namespace FarseerPhysics.Dynamics.Joints
_u = Vector2.Zero;
}
float crAu = MathUtils.Cross(_rA, _u);
float crBu = MathUtils.Cross(_rB, _u);
float crAu = MathUtils.Cross(ref _rA, ref _u);
float crBu = MathUtils.Cross(ref _rB, ref _u);
float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
// Compute the effective mass matrix.
@@ -216,7 +223,7 @@ namespace FarseerPhysics.Dynamics.Joints
float C = length - Length;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
float omega = 2.0f * MathHelper.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
@@ -239,16 +246,16 @@ namespace FarseerPhysics.Dynamics.Joints
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
wB += _invIB * MathUtils.Cross(ref _rB, ref P);
}
else
{
@@ -269,8 +276,8 @@ namespace FarseerPhysics.Dynamics.Joints
float wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
Vector2 vpA = vA + MathUtils.Cross(wA, ref _rA);
Vector2 vpB = vB + MathUtils.Cross(wB, ref _rB);
float Cdot = Vector2.Dot(_u, vpB - vpA);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
@@ -278,9 +285,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
wB += _invIB * MathUtils.Cross(ref _rB, ref P);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
@@ -302,10 +309,11 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
Vector2 u = cB + rB - cA - rA;
float length = u.Length(); u.Normalize();
@@ -316,9 +324,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = impulse * u;
cA -= _invMassA * P;
aA -= _invIA * MathUtils.Cross(rA, P);
aA -= _invIA * MathUtils.Cross(ref rA, ref P);
cB += _invMassB * P;
aB += _invIB * MathUtils.Cross(rB, P);
aB += _invIB * MathUtils.Cross(ref rB, ref P);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;