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

(77d1794a) Tester's build January 10th, 2020

Browse Source
This commit is contained in:
juanjp600
2020-01-10 14:42:38 -03:00
parent c02da46ef5
commit e6a08d715b
4529 changed files with 1145046 additions and 218950 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs
+5
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
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;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs
+14 -8
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
@@ -22,6 +27,7 @@
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -81,7 +87,7 @@ namespace FarseerPhysics.Dynamics.Joints
Debug.Assert(worldAnchor.IsValid());
_worldAnchor = worldAnchor;
LocalAnchorA = MathUtils.MulT(BodyA._xf, worldAnchor);
LocalAnchorA = Transform.Divide(ref worldAnchor, ref BodyA._xf);
}
/// <summary>
@@ -168,12 +174,12 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Rot qA = new Rot(aA);
Complex qA = Complex.FromAngle(aA);
float mass = BodyA.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
float omega = 2.0f * MathHelper.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
@@ -195,7 +201,7 @@ namespace FarseerPhysics.Dynamics.Joints
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
@@ -213,11 +219,11 @@ namespace FarseerPhysics.Dynamics.Joints
// Cheat with some damping
wA *= 0.98f;
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(_rA, _impulse);
wA += _invIA * MathUtils.Cross(ref _rA, ref _impulse);
}
else
{
@@ -234,7 +240,7 @@ namespace FarseerPhysics.Dynamics.Joints
float wA = data.velocities[_indexA].w;
// Cdot = v + cross(w, r)
Vector2 Cdot = vA + MathUtils.Cross(wA, _rA);
Vector2 Cdot = vA + MathUtils.Cross(wA, ref _rA);
Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
Vector2 oldImpulse = _impulse;
@@ -247,7 +253,7 @@ namespace FarseerPhysics.Dynamics.Joints
impulse = _impulse - oldImpulse;
vA += _invMassA * impulse;
wA += _invIA * MathUtils.Cross(_rA, impulse);
wA += _invIA * MathUtils.Cross(ref _rA, ref impulse);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs
+17 -10
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
@@ -21,6 +26,7 @@
*/
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -151,11 +157,12 @@ 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);
// Compute the effective mass matrix.
_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);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
@@ -183,7 +190,7 @@ namespace FarseerPhysics.Dynamics.Joints
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
@@ -191,9 +198,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
wB += iB * (MathUtils.Cross(ref _rB, ref P) + _angularImpulse);
}
else
{
@@ -235,9 +242,9 @@ namespace FarseerPhysics.Dynamics.Joints
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
Vector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot);
Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
@@ -252,10 +259,10 @@ namespace FarseerPhysics.Dynamics.Joints
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
wB += iB * MathUtils.Cross(ref _rB, ref impulse);
}
data.velocities[_indexA].v = vA;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs
+39 -27
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
@@ -22,6 +27,7 @@
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -148,7 +154,7 @@ namespace FarseerPhysics.Dynamics.Joints
_localAxisC = prismatic.LocalXAxis;
Vector2 pC = _localAnchorC;
Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
Vector2 pA = Complex.Divide(Complex.Multiply(ref _localAnchorA, ref xfA.q) + (xfA.p - xfC.p), ref xfC.q);
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
@@ -180,7 +186,7 @@ namespace FarseerPhysics.Dynamics.Joints
_localAxisD = prismatic.LocalXAxis;
Vector2 pD = _localAnchorD;
Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
Vector2 pB = Complex.Divide(Complex.Multiply(ref _localAnchorB, ref xfB.q) + (xfB.p - xfD.p), ref xfD.q);
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
@@ -271,7 +277,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 vD = data.velocities[_indexD].v;
float wD = data.velocities[_indexD].w;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
Complex qC = Complex.FromAngle(aC);
Complex qD = Complex.FromAngle(aD);
_mass = 0.0f;
@@ -284,12 +293,12 @@ namespace FarseerPhysics.Dynamics.Joints
}
else
{
Vector2 u = MathUtils.Mul(qC, _localAxisC);
Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
Vector2 u = Complex.Multiply(ref _localAxisC, ref qC);
Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC);
Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA);
_JvAC = u;
_JwC = MathUtils.Cross(rC, u);
_JwA = MathUtils.Cross(rA, u);
_JwC = MathUtils.Cross(ref rC, ref u);
_JwA = MathUtils.Cross(ref rA, ref u);
_mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
}
@@ -302,19 +311,19 @@ namespace FarseerPhysics.Dynamics.Joints
}
else
{
Vector2 u = MathUtils.Mul(qD, _localAxisD);
Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
Vector2 u = Complex.Multiply(ref _localAxisD, ref qD);
Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD);
Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB);
_JvBD = _ratio * u;
_JwD = _ratio * MathUtils.Cross(rD, u);
_JwB = _ratio * MathUtils.Cross(rB, u);
_JwD = _ratio * MathUtils.Cross(ref rD, ref u);
_JwB = _ratio * MathUtils.Cross(ref rB, ref u);
_mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
}
// Compute effective mass.
_mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
vA += (_mA * _impulse) * _JvAC;
wA += _iA * _impulse * _JwA;
@@ -387,7 +396,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 cD = data.positions[_indexD].c;
float aD = data.positions[_indexD].a;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
Complex qC = Complex.FromAngle(aC);
Complex qD = Complex.FromAngle(aD);
const float linearError = 0.0f;
@@ -408,16 +420,16 @@ namespace FarseerPhysics.Dynamics.Joints
}
else
{
Vector2 u = MathUtils.Mul(qC, _localAxisC);
Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
Vector2 u = Complex.Multiply(ref _localAxisC, ref qC);
Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC);
Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA);
JvAC = u;
JwC = MathUtils.Cross(rC, u);
JwA = MathUtils.Cross(rA, u);
JwC = MathUtils.Cross(ref rC, ref u);
JwA = MathUtils.Cross(ref rA, ref u);
mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
Vector2 pC = _localAnchorC - _lcC;
Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
Vector2 pA = Complex.Divide(rA + (cA - cC), ref qC);
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
@@ -432,16 +444,16 @@ namespace FarseerPhysics.Dynamics.Joints
}
else
{
Vector2 u = MathUtils.Mul(qD, _localAxisD);
Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
Vector2 u = Complex.Multiply(ref _localAxisD, ref qD);
Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD);
Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB);
JvBD = _ratio * u;
JwD = _ratio * MathUtils.Cross(rD, u);
JwB = _ratio * MathUtils.Cross(rB, u);
JwD = _ratio * MathUtils.Cross(ref rD, ref u);
JwB = _ratio * MathUtils.Cross(ref rB, ref u);
mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
Vector2 pD = _localAnchorD - _lcD;
Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
Vector2 pB = Complex.Divide(rB + (cB - cD), ref qD);
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs
+6 -1
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
@@ -162,7 +167,7 @@ namespace FarseerPhysics.Dynamics.Joints
/// Set the user data pointer.
/// </summary>
/// <value>The data.</value>
public object UserData { get; set; }
public object Tag;
/// <summary>
/// Set this flag to true if the attached bodies should collide.
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/JointFactory.cs
+124
View File
@@ -0,0 +1,124 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
using FarseerPhysics.Dynamics;
using FarseerPhysics.Dynamics.Joints;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// An easy to use factory for using joints.
/// </summary>
public static class JointFactory
{
public static MotorJoint CreateMotorJoint(World world, Body bodyA, Body bodyB, bool useWorldCoordinates = false)
{
MotorJoint joint = new MotorJoint(bodyA, bodyB, useWorldCoordinates);
world.Add(joint);
return joint;
}
public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
{
RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
world.Add(joint);
return joint;
}
public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchor)
{
Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor));
RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, anchor);
world.Add(joint);
return joint;
}
public static RopeJoint CreateRopeJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
{
RopeJoint ropeJoint = new RopeJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
world.Add(ropeJoint);
return ropeJoint;
}
public static WeldJoint CreateWeldJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
{
WeldJoint weldJoint = new WeldJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
world.Add(weldJoint);
return weldJoint;
}
public static PrismaticJoint CreatePrismaticJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
{
PrismaticJoint joint = new PrismaticJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates);
world.Add(joint);
return joint;
}
public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
{
WheelJoint joint = new WheelJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates);
world.Add(joint);
return joint;
}
public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 axis)
{
return CreateWheelJoint(world, bodyA, bodyB, Vector2.Zero, axis);
}
public static AngleJoint CreateAngleJoint(World world, Body bodyA, Body bodyB)
{
AngleJoint angleJoint = new AngleJoint(bodyA, bodyB);
world.Add(angleJoint);
return angleJoint;
}
public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
{
DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
world.Add(distanceJoint);
return distanceJoint;
}
public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB)
{
return CreateDistanceJoint(world, bodyA, bodyB, Vector2.Zero, Vector2.Zero);
}
public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
{
FrictionJoint frictionJoint = new FrictionJoint(bodyA, bodyB, anchor, useWorldCoordinates);
world.Add(frictionJoint);
return frictionJoint;
}
public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB)
{
return CreateFrictionJoint(world, bodyA, bodyB, Vector2.Zero);
}
public static GearJoint CreateGearJoint(World world, Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio)
{
GearJoint gearJoint = new GearJoint(bodyA, bodyB, jointA, jointB, ratio);
world.Add(gearJoint);
return gearJoint;
}
public static PulleyJoint CreatePulleyJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false)
{
PulleyJoint pulleyJoint = new PulleyJoint(bodyA, bodyB, anchorA, anchorB, worldAnchorA, worldAnchorB, ratio, useWorldCoordinates);
world.Add(pulleyJoint);
return pulleyJoint;
}
public static FixedMouseJoint CreateFixedMouseJoint(World world, Body body, Vector2 worldAnchor)
{
FixedMouseJoint joint = new FixedMouseJoint(body, worldAnchor);
world.Add(joint);
return joint;
}
}
}
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs
+17 -11
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
@@ -22,6 +27,7 @@
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -193,12 +199,12 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, -_localCenterA);
_rB = MathUtils.Mul(qB, -_localCenterB);
_rA = -Complex.Multiply(ref _localCenterA, ref qA);
_rB = -Complex.Multiply(ref _localCenterB, ref qB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
@@ -226,10 +232,10 @@ namespace FarseerPhysics.Dynamics.Joints
_angularMass = 1.0f / _angularMass;
}
_linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
_linearError = cB + _rB - cA - _rA - Complex.Multiply(ref _linearOffset, ref qA);
_angularError = aB - aA - _angularOffset;
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
@@ -238,9 +244,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
wB += iB * (MathUtils.Cross(ref _rB, ref P) + _angularImpulse);
}
else
{
@@ -283,7 +289,7 @@ namespace FarseerPhysics.Dynamics.Joints
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError;
Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA) + inv_h * CorrectionFactor * _linearError;
Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
Vector2 oldImpulse = _linearImpulse;
@@ -300,10 +306,10 @@ namespace FarseerPhysics.Dynamics.Joints
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
wB += iB * MathUtils.Cross(ref _rB, ref impulse);
}
data.velocities[_indexA].v = vA;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs
+35 -27
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
@@ -98,6 +104,7 @@ namespace FarseerPhysics.Dynamics.Joints
/// </summary>
public class PrismaticJoint : Joint
{
private Vector2 _localXAxis;
private Vector2 _localYAxisA;
private Vector3 _impulse;
private float _lowerTranslation;
@@ -207,7 +214,7 @@ namespace FarseerPhysics.Dynamics.Joints
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
Vector2 axis = BodyA.GetWorldVector(ref _localXAxis);
return Vector2.Dot(d, axis);
}
@@ -221,23 +228,22 @@ namespace FarseerPhysics.Dynamics.Joints
{
get
{
Transform xf1, xf2;
BodyA.GetTransform(out xf1);
BodyB.GetTransform(out xf2);
Transform xf1 = BodyA.GetTransform();
Transform xf2 = BodyB.GetTransform();
Vector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter);
Vector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter);
Vector2 r1 = Complex.Multiply(LocalAnchorA - BodyA.LocalCenter, ref xf1.q);
Vector2 r2 = Complex.Multiply(LocalAnchorB - BodyB.LocalCenter, ref xf2.q);
Vector2 p1 = BodyA._sweep.C + r1;
Vector2 p2 = BodyB._sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
Vector2 axis = BodyA.GetWorldVector(ref _localXAxis);
Vector2 v1 = BodyA._linearVelocity;
Vector2 v2 = BodyB._linearVelocity;
float w1 = BodyA._angularVelocity;
float w2 = BodyB._angularVelocity;
float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
float speed = Vector2.Dot(d, MathUtils.Cross(w1, ref axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, ref r2) - v1 - MathUtils.Cross(w1, ref r1));
return speed;
}
}
@@ -380,16 +386,16 @@ namespace FarseerPhysics.Dynamics.Joints
set
{
_axis1 = value;
LocalXAxis = BodyA.GetLocalVector(_axis1);
LocalXAxis.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, LocalXAxis);
_localXAxis = BodyA.GetLocalVector(_axis1);
_localXAxis.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, ref _localXAxis);
}
}
/// <summary>
/// The axis in local coordinates relative to BodyA
/// </summary>
public Vector2 LocalXAxis { get; private set; }
public Vector2 LocalXAxis { get { return _localXAxis; } }
/// <summary>
/// The reference angle.
@@ -427,11 +433,12 @@ 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);
// Compute the effective masses.
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 d = (cB - cA) + rB - rA;
float mA = _invMassA, mB = _invMassB;
@@ -439,9 +446,9 @@ namespace FarseerPhysics.Dynamics.Joints
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_axis = Complex.Multiply(ref _localXAxis, ref qA);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_a2 = MathUtils.Cross(ref rB, ref _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
@@ -452,10 +459,10 @@ namespace FarseerPhysics.Dynamics.Joints
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_perp = Complex.Multiply(ref _localYAxisA, ref qA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
_s2 = MathUtils.Cross(ref rB, ref _perp);
float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
float k12 = iA * _s1 + iB * _s2;
@@ -515,7 +522,7 @@ namespace FarseerPhysics.Dynamics.Joints
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
@@ -647,23 +654,24 @@ 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);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
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 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
Vector2 axis = Complex.Multiply(ref _localXAxis, ref qA);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float a2 = MathUtils.Cross(ref rB, ref axis);
Vector2 perp = Complex.Multiply(ref _localYAxisA, ref qA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
float s2 = MathUtils.Cross(ref rB, ref perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs
+27 -19
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
@@ -225,10 +231,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);
// Get the pulley axes.
_uA = cA + _rA - WorldAnchorA;
@@ -256,8 +263,8 @@ namespace FarseerPhysics.Dynamics.Joints
}
// Compute effective mass.
float ruA = MathUtils.Cross(_rA, _uA);
float ruB = MathUtils.Cross(_rB, _uB);
float ruA = MathUtils.Cross(ref _rA, ref _uA);
float ruB = MathUtils.Cross(ref _rB, ref _uB);
float mA = _invMassA + _invIA * ruA * ruA;
float mB = _invMassB + _invIB * ruB * ruB;
@@ -269,7 +276,7 @@ namespace FarseerPhysics.Dynamics.Joints
_mass = 1.0f / _mass;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= data.step.dtRatio;
@@ -279,9 +286,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 PB = (-Ratio * _impulse) * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
wA += _invIA * MathUtils.Cross(ref _rA, ref PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
wB += _invIB * MathUtils.Cross(ref _rB, ref PB);
}
else
{
@@ -301,8 +308,8 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
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(_uA, vpA) - Ratio * Vector2.Dot(_uB, vpB);
float impulse = -_mass * Cdot;
@@ -311,9 +318,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 PA = -impulse * _uA;
Vector2 PB = -Ratio * impulse * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
wA += _invIA * MathUtils.Cross(ref _rA, ref PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
wB += _invIB * MathUtils.Cross(ref _rB, ref PB);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
@@ -328,10 +335,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);
// Get the pulley axes.
Vector2 uA = cA + rA - WorldAnchorA;
@@ -359,8 +367,8 @@ namespace FarseerPhysics.Dynamics.Joints
}
// Compute effective mass.
float ruA = MathUtils.Cross(rA, uA);
float ruB = MathUtils.Cross(rB, uB);
float ruA = MathUtils.Cross(ref rA, ref uA);
float ruB = MathUtils.Cross(ref rB, ref uB);
float mA = _invMassA + _invIA * ruA * ruA;
float mB = _invMassB + _invIB * ruB * ruB;
@@ -381,9 +389,9 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 PB = -Ratio * impulse * uB;
cA += _invMassA * PA;
aA += _invIA * MathUtils.Cross(rA, PA);
aA += _invIA * MathUtils.Cross(ref rA, ref PA);
cB += _invMassB * PB;
aB += _invIB * MathUtils.Cross(rB, PB);
aB += _invIB * MathUtils.Cross(ref rB, ref PB);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs
+25 -19
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
@@ -22,6 +27,7 @@
using System;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -322,10 +328,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);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
@@ -396,7 +403,7 @@ namespace FarseerPhysics.Dynamics.Joints
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
@@ -405,10 +412,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z);
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + MotorImpulse + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z);
wB += iB * (MathUtils.Cross(ref _rB, ref P) + MotorImpulse + _impulse.Z);
}
else
{
@@ -451,7 +458,7 @@ namespace FarseerPhysics.Dynamics.Joints
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 Cdot1 = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
@@ -503,25 +510,25 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
wB += iB * (MathUtils.Cross(ref _rB, ref P) + impulse.Z);
}
else
{
// Solve point-to-point constraint
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
wB += iB * MathUtils.Cross(ref _rB, ref impulse);
}
data.velocities[_indexA].v = vA;
@@ -537,7 +544,6 @@ 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);
float angularError = 0.0f;
float positionError;
@@ -582,10 +588,10 @@ namespace FarseerPhysics.Dynamics.Joints
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
Vector2 C = cB + rB - cA - rA;
positionError = C.Length();
@@ -602,10 +608,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
aA -= iA * MathUtils.Cross(ref rA, ref impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
aB += iB * MathUtils.Cross(ref rB, ref impulse);
}
data.positions[_indexA].c = cA;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs
+26 -18
View File
@@ -1,4 +1,9 @@
/*
/* 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
*
@@ -22,6 +27,7 @@
using System;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -159,10 +165,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;
_length = _u.Length();
@@ -190,22 +197,22 @@ namespace FarseerPhysics.Dynamics.Joints
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float crA = MathUtils.Cross(ref _rA, ref _u);
float crB = MathUtils.Cross(ref _rB, ref _u);
float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 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
{
@@ -226,8 +233,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 C = _length - MaxLength;
float Cdot = Vector2.Dot(_u, vpB - vpA);
@@ -244,9 +251,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;
@@ -261,10 +268,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();
@@ -276,9 +284,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;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs
+50 -26
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
@@ -22,6 +27,7 @@
using System;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -68,11 +74,6 @@ namespace FarseerPhysics.Dynamics.Joints
private float _invIB;
private Mat33 _mass;
/// <summary>
/// If true, body B is treated as if it was kinematic (i.e. as if it had infinite mass)
/// </summary>
public bool KinematicBodyB;
internal WeldJoint()
{
JointType = JointType.Weld;
@@ -116,6 +117,11 @@ namespace FarseerPhysics.Dynamics.Joints
/// </summary>
public Vector2 LocalAnchorB { get; set; }
/// <summary>
/// If true, body B is treated as if it was kinematic (i.e. as if it had infinite mass)
/// </summary>
public bool KinematicBodyB;
public override Vector2 WorldAnchorA
{
get { return BodyA.GetWorldPoint(LocalAnchorA); }
@@ -165,6 +171,7 @@ namespace FarseerPhysics.Dynamics.Joints
_invMassA = BodyA._invMass;
_invMassB = KinematicBodyB ? 0.0f : BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
_invIB = KinematicBodyB ? 0.0f : BodyB._invI;
float aA = data.positions[_indexA].a;
@@ -175,10 +182,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);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
@@ -213,7 +221,7 @@ namespace FarseerPhysics.Dynamics.Joints
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
float omega = 2.0f * MathHelper.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
@@ -230,6 +238,12 @@ namespace FarseerPhysics.Dynamics.Joints
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else if (K.ez.Z == 0.0f)
{
K.GetInverse22(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
@@ -237,7 +251,7 @@ namespace FarseerPhysics.Dynamics.Joints
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
@@ -245,10 +259,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z);
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z);
wB += iB * (MathUtils.Cross(ref _rB, ref P) + _impulse.Z);
}
else
{
@@ -281,7 +295,7 @@ namespace FarseerPhysics.Dynamics.Joints
wA -= iA * impulse2;
wB += iB * impulse2;
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 Cdot1 = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1);
_impulse.X += impulse1.X;
@@ -290,14 +304,14 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = impulse1;
vA -= mA * P;
wA -= iA * MathUtils.Cross(_rA, P);
wA -= iA * MathUtils.Cross(ref _rA, ref P);
vB += mB * P;
wB += iB * MathUtils.Cross(_rB, P);
wB += iB * MathUtils.Cross(ref _rB, ref P);
}
else
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 Cdot1 = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
@@ -307,10 +321,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
wB += iB * (MathUtils.Cross(ref _rB, ref P) + impulse.Z);
}
data.velocities[_indexA].v = vA;
@@ -326,13 +340,14 @@ 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);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
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);
float positionError, angularError;
@@ -357,10 +372,10 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
aB += iB * MathUtils.Cross(ref rB, ref P);
}
else
{
@@ -372,14 +387,23 @@ namespace FarseerPhysics.Dynamics.Joints
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector3 impulse;
if (K.ez.Z <= 0.0f)
{
Vector2 impulse2 = -K.Solve22(C1);
impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f);
}
else
{
impulse = -K.Solve33(C);
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z);
}
data.positions[_indexA].c = cA;
Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs
+27 -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
@@ -22,6 +27,7 @@
using System;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
@@ -52,6 +58,7 @@ namespace FarseerPhysics.Dynamics.Joints
public class WheelJoint : Joint
{
// Solver shared
private Vector2 _localXAxis;
private Vector2 _localYAxis;
private float _impulse;
@@ -147,15 +154,15 @@ namespace FarseerPhysics.Dynamics.Joints
set
{
_axis = value;
LocalXAxis = BodyA.GetLocalVector(_axis);
_localYAxis = MathUtils.Cross(1.0f, LocalXAxis);
_localXAxis = BodyA.GetLocalVector(_axis);
_localYAxis = MathUtils.Rot90(ref _localXAxis);
}
}
/// <summary>
/// The axis in local coordinates relative to BodyA
/// </summary>
public Vector2 LocalXAxis { get; private set; }
public Vector2 LocalXAxis { get { return _localXAxis; } }
/// <summary>
/// The desired motor speed in radians per second.
@@ -206,7 +213,7 @@ namespace FarseerPhysics.Dynamics.Joints
Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
Vector2 d = pB - pA;
Vector2 axis = bA.GetWorldVector(LocalXAxis);
Vector2 axis = bA.GetWorldVector(ref _localXAxis);
float translation = Vector2.Dot(d, axis);
return translation;
@@ -282,18 +289,19 @@ 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);
// Compute the effective masses.
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 d1 = cB + rB - cA - rA;
// Point to line constraint
{
_ay = MathUtils.Mul(qA, _localYAxis);
_ay = Complex.Multiply(ref _localYAxis, ref qA);
_sAy = MathUtils.Cross(d1 + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_sBy = MathUtils.Cross(ref rB, ref _ay);
_mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy;
@@ -309,9 +317,9 @@ namespace FarseerPhysics.Dynamics.Joints
_gamma = 0.0f;
if (Frequency > 0.0f)
{
_ax = MathUtils.Mul(qA, LocalXAxis);
_ax = Complex.Multiply(ref _localXAxis, ref qA);
_sAx = MathUtils.Cross(d1 + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
_sBx = MathUtils.Cross(ref rB, ref _ax);
float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx;
@@ -322,7 +330,7 @@ namespace FarseerPhysics.Dynamics.Joints
float C = Vector2.Dot(d1, _ax);
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
float omega = 2.0f * MathHelper.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _springMass * DampingRatio * omega;
@@ -367,7 +375,7 @@ namespace FarseerPhysics.Dynamics.Joints
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
if (data.step.warmStarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
@@ -468,16 +476,17 @@ 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 d = (cB - cA) + rB - rA;
Vector2 ay = MathUtils.Mul(qA, _localYAxis);
Vector2 ay = Complex.Multiply(ref _localYAxis, ref qA);
float sAy = MathUtils.Cross(d + rA, ay);
float sBy = MathUtils.Cross(rB, ay);
float sBy = MathUtils.Cross(ref rB, ref ay);
float C = Vector2.Dot(d, ay);