public static void mul22ToOutUnsafe(Mat33 A, Vec2 v, ref Vec2 v2)
{
Debug.Assert(v != v2);
v2.y = A.ex.y*v.x + A.ey.y*v.y;
v2.x = A.ex.x*v.x + A.ey.x*v.y;
}
internal 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, ref 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 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, ref Vec2 v2)
{
float tempx = A.ex.x*v.x + A.ey.x*v.y;
v2.y = A.ex.y*v.x + A.ey.y*v.y;
v2.x = tempx;
}
public void set(Mat33 mat)
{
Vec3 vec = mat.ex;
ex.x = vec.x;
ex.y = vec.y;
ex.z = vec.z;
Vec3 vec1 = mat.ey;
ey.x = vec1.x;
ey.y = vec1.y;
ey.z = vec1.z;
Vec3 vec2 = mat.ez;
ez.x = vec2.x;
ez.y = vec2.y;
ez.z = vec2.z;
}
// / 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);
}
// / 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);
}
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 setScaleTransform(float scale, ref Mat33 m)
{
m.ex.x = scale;
m.ey.y = scale;
}
public static void mulToOutUnsafe(Mat33 A, Vec3 v, ref Vec3 v2)
{
Debug.Assert(v2 != v);
v2.x = v.x*A.ex.x + v.y*A.ey.x + v.z*A.ez.x;
v2.y = v.x*A.ex.y + v.y*A.ey.y + v.z*A.ez.y;
v2.z = v.x*A.ex.z + v.y*A.ey.z + v.z*A.ez.z;
}
public static void mulToOut(Mat33 A, Vec3 v, ref Vec3 v2)
{
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;
v2.x = v.x*A.ex.x + v.y*A.ey.x + v.z*A.ez.x;
v2.y = tempy;
v2.z = tempz;
}