public void AddM(b2Mat22 m)
{
col1.x += m.col1.x;
col1.y += m.col1.y;
col2.x += m.col2.x;
col2.y += m.col2.y;
}
public b2Mat22 Copy()
{
b2Mat22 mat = new b2Mat22();
mat.SetM(this);
return(mat);
}
public static b2Mat22 FromAngle(float angle)
{
b2Mat22 mat = new b2Mat22();
mat.Set(angle);
return(mat);
}
public static b2Mat22 FromVV(b2Vec2 c1, b2Vec2 c2)
{
b2Mat22 mat = new b2Mat22();
mat.SetVV(c1, c2);
return(mat);
}
public void MulTM(b2Mat22 A)
{
float tX = b2Math.Dot(this, A.col1);
y = b2Math.Dot(this, A.col2);
x = tX;
}
public void MulM(b2Mat22 A)
{
float tX = x;
x = A.col1.x * tX + A.col2.x * y;
y = A.col1.y * tX + A.col2.y * y;
}
static public b2Vec2 MulTMV(b2Mat22 A, b2Vec2 v)
{
// (tVec.x * tMat.col1.x + tVec.y * tMat.col1.y)
// (tVec.x * tMat.col2.x + tVec.y * tMat.col2.y)
b2Vec2 u = new b2Vec2(Dot(v, A.col1), Dot(v, A.col2));
return(u);
}
static public b2Vec2 MulMV(b2Mat22 A, b2Vec2 v)
{
// (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y)
// (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y)
b2Vec2 u = new b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
return(u);
}
// A^T * B
static public b2Mat22 MulTMM(b2Mat22 A, b2Mat22 B)
{
b2Vec2 c1 = new b2Vec2(Dot(A.col1, B.col1), Dot(A.col2, B.col1));
b2Vec2 c2 = new b2Vec2(Dot(A.col1, B.col2), Dot(A.col2, B.col2));
b2Mat22 C = b2Mat22.FromVV(c1, c2);
return(C);
}
/**
* The default constructor does nothing (for performance).
*/
public b2Transform(b2Vec2 pos = null, b2Mat22 r = null)
{
if (pos != null)
{
position.SetV(pos);
R.SetM(r);
}
}
/**
* Compute the inverse of this matrix, such that inv(A) * A = identity.
*/
public b2Mat22 GetInverse(b2Mat22 outM)
{
float a = col1.x;
float b = col2.x;
float c = col1.y;
float d = col2.y;
//b2Mat22 B = new b2Mat22();
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
outM.col1.x = det * d; outM.col2.x = -det * b;
outM.col1.y = -det * c; outM.col2.y = det * a;
return(outM);
}
static public b2Mat22 AbsM(b2Mat22 A)
{
b2Mat22 B = b2Mat22.FromVV(AbsV(A.col1), AbsV(A.col2));
return(B);
}
// A * B
static public b2Mat22 MulMM(b2Mat22 A, b2Mat22 B)
{
b2Mat22 C = b2Mat22.FromVV(MulMV(A, B.col1), MulMV(A, B.col2));
return(C);
}
static public b2Mat22 AddMM(b2Mat22 A, b2Mat22 B)
{
b2Mat22 C = b2Mat22.FromVV(AddVV(A.col1, B.col1), AddVV(A.col2, B.col2));
return(C);
}
public void SetM(b2Mat22 m)
{
col1.SetV(m.col1);
col2.SetV(m.col2);
}
/**
* Initialize using a position vector and a rotation matrix.
*/
public void Initialize(b2Vec2 pos, b2Mat22 r)
{
position.SetV(pos);
R.SetM(r);
}