public Bone(string name)
{
m_refPosition = new Vector2D(0.0, 0.0);
m_mat = Matrix3D.Identity.Clone();
m_totalMat = Matrix3D.Identity.Clone();
m_name = name;
}
/// <summary>
/// Subtracts a matrix from a matrix.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance to subtract from.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance to subtract.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the difference.</returns>
/// <remarks>result[x][y] = left[x][y] - right[x][y]</remarks>
public static Matrix3D Subtract(Matrix3D left, Matrix3D right)
{
return new Matrix3D(
left.M11 - right.M11, left.M12 - right.M12, left.M13 - right.M13,
left.M21 - right.M21, left.M22 - right.M22, left.M23 - right.M23,
left.M31 - right.M31, left.M32 - right.M32, left.M33 - right.M33
);
}
/// <summary>
/// Adds two matrices and put the result in a third matrix.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <param name="result">A <see cref="Matrix3D"/> instance to hold the result.</param>
public static void Add(Matrix3D left, Matrix3D right, ref Matrix3D result)
{
result.M11 = left.M11 + right.M11;
result.M12 = left.M12 + right.M12;
result.M13 = left.M13 + right.M13;
result.M21 = left.M21 + right.M21;
result.M22 = left.M22 + right.M22;
result.M23 = left.M23 + right.M23;
result.M31 = left.M31 + right.M31;
result.M32 = left.M32 + right.M32;
result.M33 = left.M33 + right.M33;
}
/// <summary>
/// Adds two matrices.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the sum.</returns>
public static Matrix3D Add(Matrix3D left, Matrix3D right)
{
return new Matrix3D(
left.M11 + right.M11, left.M12 + right.M12, left.M13 + right.M13,
left.M21 + right.M21, left.M22 + right.M22, left.M23 + right.M23,
left.M31 + right.M31, left.M32 + right.M32, left.M33 + right.M33
);
}
/// <summary>
/// Initializes a new instance of the <see cref="Matrix3D"/> class using a given matrix.
/// </summary>
public Matrix3D(Matrix3D m)
{
_m11 = m.M11; _m12 = m.M12; _m13 = m.M13;
_m21 = m.M21; _m22 = m.M22; _m23 = m.M23;
_m31 = m.M31; _m32 = m.M32; _m33 = m.M33;
}
public void SetLocalMatrix(Matrix3D mat3D)
{
m_mat = mat3D.Clone();
}
/// <summary>
/// Adds a matrix and a scalar.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="scalar">A double-precision floating-point number.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the sum.</returns>
public static Matrix3D operator +(double scalar, Matrix3D matrix)
{
return(Matrix3D.Add(matrix, scalar));
}
/// <summary>
/// Transforms a given vector by a matrix.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="vector">A <see cref="Vector3D"/> instance.</param>
/// <returns>A new <see cref="Vector3D"/> instance containing the result.</returns>
public static Vector3D Transform(Matrix3D matrix, Vector3D vector)
{
return new Vector3D(
(matrix.M11 * vector.X) + (matrix.M12 * vector.Y) + (matrix.M13 * vector.Z),
(matrix.M21 * vector.X) + (matrix.M22 * vector.Y) + (matrix.M23 * vector.Z),
(matrix.M31 * vector.X) + (matrix.M32 * vector.Y) + (matrix.M33 * vector.Z));
}
public Transform2D(Matrix3D mat)
{
_mat = mat.Clone();
}
public Transform2D(Transform2D transf)
{
_mat = transf._mat.Clone();
}
public Transform2D(Matrix3D mat)
{
_mat = mat.Clone();
}
/// <summary>
/// Multiplies two matrices.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the result.</returns>
public static Matrix3D operator *(Matrix3D left, Matrix3D right)
{
return(Matrix3D.Multiply(left, right));
}
/// <summary>
/// Subtracts a scalar from a matrix.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="scalar">A double-precision floating-point number.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the difference.</returns>
public static Matrix3D operator -(Matrix3D matrix, double scalar)
{
return(Matrix3D.Subtract(matrix, scalar));
}
/// <summary>
/// Subtracts a matrix from a matrix.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the difference.</returns>
public static Matrix3D operator -(Matrix3D left, Matrix3D right)
{
return(Matrix3D.Subtract(left, right));;
}
/// <summary>
/// Subtracts a matrix from a matrix and put the result in a third matrix.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance to subtract from.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance to subtract.</param>
/// <param name="result">A <see cref="Matrix3D"/> instance to hold the result.</param>
/// <remarks>result[x][y] = left[x][y] - right[x][y]</remarks>
public static void Subtract(Matrix3D left, Matrix3D right, ref Matrix3D result)
{
result.M11 = left.M11 - right.M11;
result.M12 = left.M12 - right.M12;
result.M13 = left.M13 - right.M13;
result.M21 = left.M21 - right.M21;
result.M22 = left.M22 - right.M22;
result.M23 = left.M23 - right.M23;
result.M31 = left.M31 - right.M31;
result.M32 = left.M32 - right.M32;
result.M33 = left.M33 - right.M33;
}
/// <summary>
/// Multiplies two matrices.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the result.</returns>
public static Matrix3D Multiply(Matrix3D left, Matrix3D right)
{
return new Matrix3D(
left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31,
left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32,
left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33,
left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31,
left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32,
left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33,
left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31,
left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32,
left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33
);
}
public Transform2D(Transform2D transf)
{
_mat = transf._mat.Clone();
}
/// <summary>
/// Transposes a matrix.
/// </summary>
/// <param name="m">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the transposed matrix.</returns>
public static Matrix3D Transpose(Matrix3D m)
{
Matrix3D t = new Matrix3D(m);
t.Transpose();
return t;
}
public Transform2D Inverse()
{
return(new Transform2D(Matrix3D.Inverse(_mat)));
}
public void ContactLocalMatrixRight(Matrix3D mat3D)
{
m_mat = Matrix3D.Multiply(m_mat, mat3D);
}
/// <summary>
/// Initializes a new instance of the <see cref="Matrix3D"/> class using a given matrix.
/// </summary>
public Matrix3D(Matrix3D m)
{
_m11 = m.M11; _m12 = m.M12; _m13 = m.M13;
_m21 = m.M21; _m22 = m.M22; _m23 = m.M23;
_m31 = m.M31; _m32 = m.M32; _m33 = m.M33;
}
//����������ȫ�ֱ任���ݵ��ӹ����ϡ�
public void Update(Bone parent)
{
if(parent == null)
{
m_totalMat = m_mat;
return;
}
Vector2D refPosDiff = m_refPosition - parent.m_refPosition;
//transform and then rotate
Matrix3D transMat = Matrix.Transform2D(refPosDiff.X, refPosDiff.Y);
m_totalMat = Matrix3D.Multiply(parent.m_totalMat, transMat);
//contact rotate matrix
m_totalMat = Matrix3D.Multiply(m_totalMat, m_mat);
}
/// <summary>
/// Transforms a given vector by a matrix and put the result in a vector.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="vector">A <see cref="Vector3D"/> instance.</param>
/// <param name="result">A <see cref="Vector3D"/> instance to hold the result.</param>
public static void Transform(Matrix3D matrix, Vector3D vector, ref Vector3D result)
{
result.X = (matrix.M11 * vector.X) + (matrix.M12 * vector.Y) + (matrix.M13 * vector.Z);
result.Y = (matrix.M21 * vector.X) + (matrix.M22 * vector.Y) + (matrix.M23 * vector.Z);
result.Z = (matrix.M31 * vector.X) + (matrix.M32 * vector.Y) + (matrix.M33 * vector.Z);
}
/// <summary>
/// Converts the specified string to its <see cref="Matrix3D"/> equivalent.
/// A return value indicates whether the conversion succeeded or failed.
/// </summary>
/// <param name="value">A string representation of a <see cref="Matrix3D"/>.</param>
/// <param name="result">
/// When this method returns, if the conversion succeeded,
/// contains a <see cref="Matrix3D"/> representing the vector specified by <paramref name="value"/>.
/// </param>
/// <returns><see langword="true"/> if value was converted successfully; otherwise, <see langword="false"/>.</returns>
/// <remarks>
/// The string should be in the following form: "3x3..matrix elements..>".<br/>
/// Exmaple : "3x3[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16]"
/// </remarks>
public static bool TryParse(string value, out Matrix3D result)
{
Regex r = new Regex(regularExp, RegexOptions.Singleline);
Match m = r.Match(value);
if (m.Success)
{
result = new Matrix3D(
double.Parse(m.Result("${m11}")),
double.Parse(m.Result("${m12}")),
double.Parse(m.Result("${m13}")),
double.Parse(m.Result("${m21}")),
double.Parse(m.Result("${m22}")),
double.Parse(m.Result("${m23}")),
double.Parse(m.Result("${m31}")),
double.Parse(m.Result("${m32}")),
double.Parse(m.Result("${m33}"))
);
return true;
}
result = Matrix3D.Zero;
return false;
}
/// <summary>
/// Adds two matrices.
/// </summary>
/// <param name="a">A <see cref="Matrix3D"/> instance.</param>
/// <param name="b">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the sum.</returns>
public static Matrix3D operator+(Matrix3D a, Matrix3D b)
{
return(Matrix3D.Add(a, b));
}
/// <summary>
/// Adds a matrix and a scalar.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="scalar">A double-precision floating-point number.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the sum.</returns>
public static Matrix3D Add(Matrix3D matrix, double scalar)
{
return new Matrix3D(
matrix.M11 + scalar, matrix.M12 + scalar, matrix.M13 + scalar,
matrix.M21 + scalar, matrix.M22 + scalar, matrix.M23 + scalar,
matrix.M31 + scalar, matrix.M32 + scalar, matrix.M33 + scalar
);
}
/// <summary>
/// Adds a matrix and a scalar.
/// </summary>
/// <param name="a">A <see cref="Matrix3D"/> instance.</param>
/// <param name="s">A scalar.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the sum.</returns>
public static Matrix3D operator+(double s, Matrix3D a)
{
return(Matrix3D.Add(a, s));
}
/// <summary>
/// Adds a matrix and a scalar and put the result in a third matrix.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="scalar">A double-precision floating-point number.</param>
/// <param name="result">A <see cref="Matrix3D"/> instance to hold the result.</param>
public static void Add(Matrix3D matrix, double scalar, ref Matrix3D result)
{
result.M11 = matrix.M11 + scalar;
result.M12 = matrix.M12 + scalar;
result.M13 = matrix.M13 + scalar;
result.M21 = matrix.M21 + scalar;
result.M22 = matrix.M22 + scalar;
result.M23 = matrix.M23 + scalar;
result.M31 = matrix.M31 + scalar;
result.M32 = matrix.M32 + scalar;
result.M33 = matrix.M33 + scalar;
}
/// <summary>
/// Subtracts a matrix from a matrix.
/// </summary>
/// <param name="a">A <see cref="Matrix3D"/> instance.</param>
/// <param name="b">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the difference.</returns>
public static Matrix3D operator-(Matrix3D a, Matrix3D b)
{
return(Matrix3D.Subtract(a, b));
}
/// <summary>
/// Subtracts a scalar from a matrix.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="scalar">A double-precision floating-point number.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the difference.</returns>
public static Matrix3D Subtract(Matrix3D matrix, double scalar)
{
return new Matrix3D(
matrix.M11 - scalar, matrix.M12 - scalar, matrix.M13 - scalar,
matrix.M21 - scalar, matrix.M22 - scalar, matrix.M23 - scalar,
matrix.M31 - scalar, matrix.M32 - scalar, matrix.M33 - scalar
);
}
/// <summary>
/// Subtracts a scalar from a matrix.
/// </summary>
/// <param name="a">A <see cref="Matrix3D"/> instance.</param>
/// <param name="s">A scalar.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the difference.</returns>
public static Matrix3D operator-(Matrix3D a, double s)
{
return(Matrix3D.Subtract(a, s));
}
/// <summary>
/// Subtracts a scalar from a matrix and put the result in a third matrix.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="scalar">A double-precision floating-point number.</param>
/// <param name="result">A <see cref="Matrix3D"/> instance to hold the result.</param>
public static void Subtract(Matrix3D matrix, double scalar, ref Matrix3D result)
{
result.M11 = matrix.M11 - scalar;
result.M12 = matrix.M12 - scalar;
result.M13 = matrix.M13 - scalar;
result.M21 = matrix.M21 - scalar;
result.M22 = matrix.M22 - scalar;
result.M23 = matrix.M23 - scalar;
result.M31 = matrix.M31 - scalar;
result.M32 = matrix.M32 - scalar;
result.M33 = matrix.M33 - scalar;
}
/// <summary>
/// Multiplies two matrices.
/// </summary>
/// <param name="a">A <see cref="Matrix3D"/> instance.</param>
/// <param name="b">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the result.</returns>
public static Matrix3D operator*(Matrix3D a, Matrix3D b)
{
return(Matrix3D.Multiply(a, b));
}
/// <summary>
/// Multiplies two matrices and put the result in a third matrix.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <param name="result">A <see cref="Matrix3D"/> instance to hold the result.</param>
public static void Multiply(Matrix3D left, Matrix3D right, ref Matrix3D result)
{
result.M11 = left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31;
result.M12 = left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32;
result.M13 = left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33;
result.M21 = left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31;
result.M22 = left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32;
result.M23 = left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33;
result.M31 = left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31;
result.M32 = left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32;
result.M33 = left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33;
}
/// <summary>
/// Transforms a given vector by a matrix.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="vector">A <see cref="Vector3D"/> instance.</param>
/// <returns>A new <see cref="Vector3D"/> instance containing the result.</returns>
public static Vector3D operator*(Matrix3D matrix, Vector3D vector)
{
return(Matrix3D.Transform(matrix, vector));
}
/// <summary>
/// Transforms a given vector by a matrix and put the result in a vector.
/// </summary>
/// <param name="matrix">A <see cref="Matrix3D"/> instance.</param>
/// <param name="vector">A <see cref="Vector3D"/> instance.</param>
/// <param name="result">A <see cref="Vector3D"/> instance to hold the result.</param>
public static void Transform(Matrix3D matrix, Vector3D vector, ref Vector3D result)
{
result.X = (matrix.M11 * vector.X) + (matrix.M12 * vector.Y) + (matrix.M13 * vector.Z);
result.Y = (matrix.M21 * vector.X) + (matrix.M22 * vector.Y) + (matrix.M23 * vector.Z);
result.Z = (matrix.M31 * vector.X) + (matrix.M32 * vector.Y) + (matrix.M33 * vector.Z);
}
public void ContactLocalMatrixLeft(Matrix3D mat3D)
{
m_mat = Matrix3D.Multiply(mat3D, m_mat);
}
public static Matrix3D Inverse(Matrix3D m)
{
double d = 1.0/m.GetDeterminant();
return new Matrix3D(
d * (m._m33 * m._m22 - m._m32 * m._m23)
, -d * (m._m33 * m._m12 - m._m32 * m._m13)
, d * (m._m23 * m._m12 - m._m22 * m._m13)
, -d * (m._m33 * m._m21 - m._m31 * m._m23)
, d * (m._m33 * m._m11 - m._m31 * m._m13)
, -d * (m._m23 * m._m11 - m._m21 * m._m13)
, d * (m._m32 * m._m21 - m._m31 * m._m22)
, -d * (m._m32 * m._m11 - m._m31 * m._m12)
, d * (m._m22 * m._m11 - m._m21 * m._m12));
}
/// <summary>
/// Adds two matrices.
/// </summary>
/// <param name="left">A <see cref="Matrix3D"/> instance.</param>
/// <param name="right">A <see cref="Matrix3D"/> instance.</param>
/// <returns>A new <see cref="Matrix3D"/> instance containing the sum.</returns>
public static Matrix3D operator +(Matrix3D left, Matrix3D right)
{
return(Matrix3D.Add(left, right));;
}
