public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
LocalAnchorA = new Vec2(def.LocalAnchorA);
LocalAnchorB = new Vec2(def.LocalAnchorB);
LocalXAxisA = new Vec2(def.LocalAxisA);
LocalXAxisA.Normalize();
LocalYAxisA = new Vec2();
Vec2.CrossToOutUnsafe(1f, LocalXAxisA, LocalYAxisA);
ReferenceAngle = def.ReferenceAngle;
Impulse = new Vec3();
MotorMass = 0.0f;
MotorImpulse = 0.0f;
LowerTranslation = def.LowerTranslation;
UpperTranslation = def.UpperTranslation;
m_maxMotorForce = def.MaxMotorForce;
m_motorSpeed = def.MotorSpeed;
m_limitEnabled = def.EnableLimit;
m_motorEnabled = def.EnableMotor;
LimitState = LimitState.Inactive;
K = new Mat33();
Axis = new Vec2();
Perp = new Vec2();
}
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_localXAxisA = new Vec2(def.localAxisA);
m_localXAxisA.normalize();
m_localYAxisA = new Vec2();
Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse = new Vec3();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
m_K = new Mat33();
m_axis = new Vec2();
m_perp = new Vec2();
}
public static void Mul22ToOutUnsafe(Mat33 a, Vec2 v, Vec2 result)
{
Debug.Assert(v != result);
result.Y = a.Ex.Y * v.X + a.Ey.Y * v.Y;
result.X = a.Ex.X * v.X + a.Ey.X * v.Y;
}
public static void MulToOut(Mat33 a, Vec3 v, Vec3 result)
{
float tempy = v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y;
float tempz = v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z;
result.X = v.X * a.Ex.X + v.Y * a.Ey.X + v.Z * a.Ez.X;
result.Y = tempy;
result.Z = tempz;
}
public static Vec2 Mul22(Mat33 a, Vec2 v)
{
return new Vec2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y);
}
public static void Mul22ToOut(Mat33 a, Vec2 v, Vec2 result)
{
float tempx = a.Ex.X * v.X + a.Ey.X * v.Y;
result.Y = a.Ex.Y * v.X + a.Ey.Y * v.Y;
result.X = tempx;
}
// / Returns the zero matrix if singular.
public void GetSymInverse33(Mat33 m)
{
float bx = Ey.Y * Ez.Z - Ey.Z * Ez.Y;
float by = Ey.Z * Ez.X - Ey.X * Ez.Z;
float bz = Ey.X * Ez.Y - Ey.Y * Ez.X;
float det = Ex.X * bx + Ex.Y * by + Ex.Z * bz;
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = Ex.X, a12 = Ey.X, a13 = Ez.X;
float a22 = Ey.Y, a23 = Ez.Y;
float a33 = Ez.Z;
m.Ex.X = det * (a22 * a33 - a23 * a23);
m.Ex.Y = det * (a13 * a23 - a12 * a33);
m.Ex.Z = det * (a12 * a23 - a13 * a22);
m.Ey.X = m.Ex.Y;
m.Ey.Y = det * (a11 * a33 - a13 * a13);
m.Ey.Z = det * (a13 * a12 - a11 * a23);
m.Ez.X = m.Ex.Z;
m.Ez.Y = m.Ey.Z;
m.Ez.Z = det * (a11 * a22 - a12 * a12);
}
// / Multiply a matrix times a vector.
public static Vec3 Mul(Mat33 a, Vec3 v)
{
return new Vec3(v.X * a.Ex.X + v.Y * a.Ey.X + v.Z + a.Ez.X, v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y, v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z);
}
public void GetInverse22(Mat33 m)
{
float a = Ex.X, b = Ey.X, c = Ex.Y, d = Ey.Y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
m.Ex.X = det * d;
m.Ey.X = (-det) * b;
m.Ex.Z = 0.0f;
m.Ex.Y = (-det) * c;
m.Ey.Y = det * a;
m.Ey.Z = 0.0f;
m.Ez.X = 0.0f;
m.Ez.Y = 0.0f;
m.Ez.Z = 0.0f;
}
public static void MulToOutUnsafe(Mat33 a, Vec3 v, Vec3 result)
{
Debug.Assert(result != v);
result.X = v.X * a.Ex.X + v.Y * a.Ey.X + v.Z * a.Ez.X;
result.Y = v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y;
result.Z = v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z;
}
public static void Mul22ToOutUnsafe(Mat33 a, Vec2 v, Vec2 result)
{
Debug.Assert(v != result);
result.Y = a.Ex.Y * v.X + a.Ey.Y * v.Y;
result.X = a.Ex.X * v.X + a.Ey.X * v.Y;
}
public static Vec2 Mul22(Mat33 a, Vec2 v)
{
return(new Vec2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y));
}
// / Multiply a matrix times a vector.
public static Vec3 Mul(Mat33 a, Vec3 v)
{
return(new Vec3(v.X * a.Ex.X + v.Y * a.Ey.X + v.Z + a.Ez.X, v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y, v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z));
}
