/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
public Vec3 Solve33(Vec3 b)
{
float det = Vec3.Dot(Col1, Vec3.Cross(Col2, Col3));
Box2DXDebug.Assert(det != 0.0f);
det = 1.0f / det;
Vec3 x = new Vec3();
x.X = det * Vec3.Dot(b, Vec3.Cross(Col2, Col3));
x.Y = det * Vec3.Dot(Col1, Vec3.Cross(b, Col3));
x.Z = det * Vec3.Dot(Col1, Vec3.Cross(Col2, b));
return x;
}
public Vec3 Solve33(Vec3 b)
{
float num = Vec3.Dot(this.Col1, Vec3.Cross(this.Col2, this.Col3));
Box2DXDebug.Assert(num != 0f);
num = 1f / num;
return new Vec3
{
X = num * Vec3.Dot(b, Vec3.Cross(this.Col2, this.Col3)),
Y = num * Vec3.Dot(this.Col1, Vec3.Cross(b, this.Col3)),
Z = num * Vec3.Dot(this.Col1, Vec3.Cross(this.Col2, b))
};
}
public RevoluteJoint(RevoluteJointDef def)
: base(def)
{
this._localAnchor1 = def.LocalAnchor1;
this._localAnchor2 = def.LocalAnchor2;
this._referenceAngle = def.ReferenceAngle;
this._impulse = default(Vec3);
this._motorImpulse = 0f;
this._lowerAngle = def.LowerAngle;
this._upperAngle = def.UpperAngle;
this._maxMotorTorque = def.MaxMotorTorque;
this._motorSpeed = def.MotorSpeed;
this._enableLimit = def.EnableLimit;
this._enableMotor = def.EnableMotor;
}
public RevoluteJoint(RevoluteJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_referenceAngle = def.ReferenceAngle;
_impulse = new Vec3();
_motorImpulse = 0.0f;
_lowerAngle = def.LowerAngle;
_upperAngle = def.UpperAngle;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
this._localCenter1 = body.GetLocalCenter();
this._localCenter2 = body2.GetLocalCenter();
XForm xForm = body.GetXForm();
XForm xForm2 = body2.GetXForm();
Vec2 v = Box2DX.Common.Math.Mul(xForm.R, this._localAnchor1 - this._localCenter1);
Vec2 vec = Box2DX.Common.Math.Mul(xForm2.R, this._localAnchor2 - this._localCenter2);
Vec2 vec2 = body2._sweep.C + vec - body._sweep.C - v;
this._invMass1 = body._invMass;
this._invI1 = body._invI;
this._invMass2 = body2._invMass;
this._invI2 = body2._invI;
this._axis = Box2DX.Common.Math.Mul(xForm.R, this._localXAxis1);
this._a1 = Vec2.Cross(vec2 + v, this._axis);
this._a2 = Vec2.Cross(vec, this._axis);
this._motorMass = this._invMass1 + this._invMass2 + this._invI1 * this._a1 * this._a1 + this._invI2 * this._a2 * this._a2;
Box2DXDebug.Assert(this._motorMass > Settings.FLT_EPSILON);
this._motorMass = 1f / this._motorMass;
this._perp = Box2DX.Common.Math.Mul(xForm.R, this._localYAxis1);
this._s1 = Vec2.Cross(vec2 + v, this._perp);
this._s2 = Vec2.Cross(vec, this._perp);
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num = invI * this._s1 + invI2 * this._s2;
float num2 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y = invI + invI2;
float num3 = invI * this._a1 + invI2 * this._a2;
float z = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num, num2);
this._K.Col2.Set(num, y, num3);
this._K.Col3.Set(num2, num3, z);
if (this._enableLimit)
{
float num4 = Vec2.Dot(this._axis, vec2);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num4 <= this._lowerTranslation)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._limitState = LimitState.AtLowerLimit;
this._impulse.Z = 0f;
}
}
else
{
if (num4 >= this._upperTranslation)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._limitState = LimitState.AtUpperLimit;
this._impulse.Z = 0f;
}
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Z = 0f;
}
}
}
}
if (!this._enableMotor)
{
this._motorImpulse = 0f;
}
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._motorImpulse *= step.DtRatio;
Vec2 v2 = this._impulse.X * this._perp + (this._motorImpulse + this._impulse.Z) * this._axis;
float num5 = this._impulse.X * this._s1 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a1;
float num6 = this._impulse.X * this._s2 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a2;
Body expr_4BB = body;
expr_4BB._linearVelocity -= this._invMass1 * v2;
body._angularVelocity -= this._invI1 * num5;
Body expr_4EF = body2;
expr_4EF._linearVelocity += this._invMass2 * v2;
body2._angularVelocity += this._invI2 * num6;
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._linearVelocity;
float num = body._angularVelocity;
Vec2 vec2 = body2._linearVelocity;
float num2 = body2._angularVelocity;
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
if (this._enableMotor && this._limitState != LimitState.EqualLimits)
{
float num3 = num2 - num - this._motorSpeed;
float num4 = this._motorMass * -num3;
float motorImpulse = this._motorImpulse;
float num5 = step.Dt * this._maxMotorTorque;
this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
num4 = this._motorImpulse - motorImpulse;
num -= invI * num4;
num2 += invI2 * num4;
}
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
float z = num2 - num;
Vec3 v2 = new Vec3(v.X, v.Y, z);
Vec3 v3 = this._mass.Solve33(-v2);
if (this._limitState == LimitState.EqualLimits)
{
this._impulse += v3;
}
else
{
if (this._limitState == LimitState.AtLowerLimit)
{
float num6 = this._impulse.Z + v3.Z;
if (num6 < 0f)
{
Vec2 vec3 = this._mass.Solve22(-v);
v3.X = vec3.X;
v3.Y = vec3.Y;
v3.Z = -this._impulse.Z;
this._impulse.X = this._impulse.X + vec3.X;
this._impulse.Y = this._impulse.Y + vec3.Y;
this._impulse.Z = 0f;
}
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
float num6 = this._impulse.Z + v3.Z;
if (num6 > 0f)
{
Vec2 vec3 = this._mass.Solve22(-v);
v3.X = vec3.X;
v3.Y = vec3.Y;
v3.Z = -this._impulse.Z;
this._impulse.X = this._impulse.X + vec3.X;
this._impulse.Y = this._impulse.Y + vec3.Y;
this._impulse.Z = 0f;
}
}
}
}
Vec2 vec4 = new Vec2(v3.X, v3.Y);
vec -= invMass * vec4;
num -= invI * (Vec2.Cross(a, vec4) + v3.Z);
vec2 += invMass2 * vec4;
num2 += invI2 * (Vec2.Cross(a2, vec4) + v3.Z);
}
else
{
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v4 = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
Vec2 vec5 = this._mass.Solve22(-v4);
this._impulse.X = this._impulse.X + vec5.X;
this._impulse.Y = this._impulse.Y + vec5.Y;
vec -= invMass * vec5;
num -= invI * Vec2.Cross(a, vec5);
vec2 += invMass2 * vec5;
num2 += invI2 * Vec2.Cross(a2, vec5);
}
body._linearVelocity = vec;
body._angularVelocity = num;
body2._linearVelocity = vec2;
body2._angularVelocity = num2;
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vec2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
//Solve motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorTorque;
_motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.EqualLimits)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vec2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpperLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vec2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vec2 P = new Vec2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z);
v2 += m2 * P;
w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z);
}
else
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
Vec2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * Vec2.Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * Vec2.Cross(r2, impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
this._mass.Col1.Y = this._mass.Col2.X;
this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
this._mass.Col3.Y = a.X * invI + a2.X * invI2;
this._mass.Col1.Z = this._mass.Col3.X;
this._mass.Col2.Z = this._mass.Col3.Y;
this._mass.Col3.Z = invI + invI2;
this._motorMass = 1f / (invI + invI2);
if (!this._enableMotor)
{
this._motorImpulse = 0f;
}
if (this._enableLimit)
{
float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num <= this._lowerAngle)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._impulse.Z = 0f;
}
this._limitState = LimitState.AtLowerLimit;
}
else
{
if (num >= this._upperAngle)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._impulse.Z = 0f;
}
this._limitState = LimitState.AtUpperLimit;
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Z = 0f;
}
}
}
}
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._motorImpulse *= step.DtRatio;
Vec2 vec = new Vec2(this._impulse.X, this._impulse.Y);
Body expr_363 = body;
expr_363._linearVelocity -= invMass * vec;
body._angularVelocity -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
Body expr_3A8 = body2;
expr_3A8._linearVelocity += invMass2 * vec;
body2._angularVelocity += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
/// <summary>
/// Construct this matrix using columns.
/// </summary>
public Mat33(Vec3 c1, Vec3 c2, Vec3 c3)
{
Col1 = c1;
Col2 = c2;
Col3 = c3;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.X * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vec2 P = new Vec2(_impulse.X, _impulse.Y);
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vec2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
Vec2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
float translation = Vec2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DX.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
Vec3 impulse;
Vec2 C1 = new Vec2();
C1.X = Vec2.Dot(_perp, d);
C1.Y = a2 - a1 - _refAngle;
linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X));
angularError = Box2DX.Common.Math.Abs(C1.Y);
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12, k13);
_K.Col2.Set(k12, k22, k23);
_K.Col3.Set(k13, k23, k33);
Vec3 C = new Vec3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = _K.Solve33(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1.Set(k11, k12, 0.0f);
_K.Col2.Set(k12, k22, 0.0f);
Vec2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vec2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vec2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorForce;
_motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vec2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
Vec2 Cdot1;
Cdot1.X = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
Cdot1.Y = w2 - w1;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 f1 = _impulse;
Vec3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState ==LimitState.AtLowerLimit)
{
_impulse.Z = Box2DX.Common.Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpperLimit)
{
_impulse.Z = Box2DX.Common.Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y);
Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vec2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vec2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vec2 P = df.X * _perp;
float L1 = df.X * _s1 + df.Y;
float L2 = df.X * _s2 + df.Y;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
public Mat33(Vec3 c1, Vec3 c2, Vec3 c3)
{
this.Col1 = c1;
this.Col2 = c2;
this.Col3 = c3;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// You cannot create a prismatic joint between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
XForm xf1 = b1.GetXForm();
XForm xf2 = b2.GetXForm();
// Compute the effective masses.
Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12, k13);
_K.Col2.Set(k12, k22, k23);
_K.Col3.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vec2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
}
public static float Dot(Vec3 a, Vec3 b)
{
return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
}
public static Vec3 Cross(Vec3 a, Vec3 b)
{
return new Vec3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
}
public static Vec3 GetOGLPos(int x, int y)
{
int[] viewport = new int[4];
double[] modelviewMatrix = new double[16];
double[] projectionMatrix = new double[16];
Gl.glGetDoublev(Gl.GL_MODELVIEW_MATRIX, modelviewMatrix);
Gl.glGetDoublev(Gl.GL_PROJECTION_MATRIX, projectionMatrix);
Gl.glGetIntegerv(Gl.GL_VIEWPORT, viewport);
float winX, winY, winZ;
float[] winz = new float[1];
winX = x;
winY = (float)viewport[3] - (float)y;
Gl.glReadPixels(x, (int)winY, 1, 1, Gl.GL_DEPTH_COMPONENT, Gl.GL_FLOAT, winz);
/*var winZ = (float)Marshal.PtrToStructure(ptr, typeof(float));
Marshal.FreeHGlobal(ptr);*/
winZ = winz[0];
double nearX, nearY, nearZ;
double farX, farY, farZ;
double posX, posY, posZ;
int success;
success = Glu.gluUnProject(winX, winY, 0.0f, modelviewMatrix, projectionMatrix, viewport,
out nearX, out nearY, out nearZ);
if (success == 0)
{
throw new ApplicationException("Failed");
}
success = Glu.gluUnProject(winX, winY, 1000, modelviewMatrix, projectionMatrix, viewport,
out farX, out farY, out farZ);
if (success == 0)
{
throw new ApplicationException("Failed");
}
posX = nearX - farX;
posY = nearY - farY;
posZ = nearZ - farY;
/* Console.WriteLine(
"X:Y {0}:{1} posX:posY:posZ {2}:{3}:{4}", x, y, posX, posY, posZ);*/
var ration = 0.0165f;
var result = new Vec3((float)posX / ration, (float)posY / ration, (float)posZ / ration);
return result;
}
private void Events_MouseMotion(object sender, MouseMotionEventArgs e)
{
this.windowMousePos = new Vec2(e.X, e.Y);
int[] viewport = new int[4];
Gl.glGetIntegerv(Gl.GL_VIEWPORT, viewport);
var wx = (float)e.X;
var wy = (float)viewport[3] - (float)e.Y;
float[] wz = new float[1];
Gl.glReadPixels((int)wx, (int)wy, 1, 1, Gl.GL_DEPTH_COMPONENT, Gl.GL_FLOAT, wz);
Console.WriteLine("wz {0}", wz[0]);
this.mousePos = Helper.GetOGLPos(e.X, e.Y);
var foo = Helper.Foo(e.X, e.Y);
Console.WriteLine("x:y:z {0}:{1}:{2}", foo.X, foo.Y, foo.Z);
foreach (var worldObject in this.worldObjects)
{
if (worldObject.HitTest(this.mousePos.X, this.mousePos.Y))
{
break;
}
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._linearVelocity;
float num = body._angularVelocity;
Vec2 vec2 = body2._linearVelocity;
float num2 = body2._angularVelocity;
if (this._enableMotor && this._limitState != LimitState.EqualLimits)
{
float num3 = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
float num4 = this._motorMass * (this._motorSpeed - num3);
float motorImpulse = this._motorImpulse;
float num5 = step.Dt * this._maxMotorForce;
this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
num4 = this._motorImpulse - motorImpulse;
Vec2 v = num4 * this._axis;
float num6 = num4 * this._a1;
float num7 = num4 * this._a2;
vec -= this._invMass1 * v;
num -= this._invI1 * num6;
vec2 += this._invMass2 * v;
num2 += this._invI2 * num7;
}
Vec2 v2;
v2.X = Vec2.Dot(this._perp, vec2 - vec) + this._s2 * num2 - this._s1 * num;
v2.Y = num2 - num;
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
float z = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
Vec3 v3 = new Vec3(v2.X, v2.Y, z);
Vec3 impulse = this._impulse;
Vec3 v4 = this._K.Solve33(-v3);
this._impulse += v4;
if (this._limitState == LimitState.AtLowerLimit)
{
this._impulse.Z = Box2DX.Common.Math.Max(this._impulse.Z, 0f);
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
this._impulse.Z = Box2DX.Common.Math.Min(this._impulse.Z, 0f);
}
}
Vec2 b = -v2 - (this._impulse.Z - impulse.Z) * new Vec2(this._K.Col3.X, this._K.Col3.Y);
Vec2 vec3 = this._K.Solve22(b) + new Vec2(impulse.X, impulse.Y);
this._impulse.X = vec3.X;
this._impulse.Y = vec3.Y;
v4 = this._impulse - impulse;
Vec2 v = v4.X * this._perp + v4.Z * this._axis;
float num6 = v4.X * this._s1 + v4.Y + v4.Z * this._a1;
float num7 = v4.X * this._s2 + v4.Y + v4.Z * this._a2;
vec -= this._invMass1 * v;
num -= this._invI1 * num6;
vec2 += this._invMass2 * v;
num2 += this._invI2 * num7;
}
else
{
Vec2 vec4 = this._K.Solve22(-v2);
this._impulse.X = this._impulse.X + vec4.X;
this._impulse.Y = this._impulse.Y + vec4.Y;
Vec2 v = vec4.X * this._perp;
float num6 = vec4.X * this._s1 + vec4.Y;
float num7 = vec4.X * this._s2 + vec4.Y;
vec -= this._invMass1 * v;
num -= this._invI1 * num6;
vec2 += this._invMass2 * v;
num2 += this._invI2 * num7;
}
body._linearVelocity = vec;
body._angularVelocity = num;
body2._linearVelocity = vec2;
body2._angularVelocity = num2;
}
