public void Read(GrnBoneNode boneNode)
{
this.ParentIndex = boneNode.ParentIndex;
this.Position = boneNode.Position;
this.Rotation = boneNode.Rotation;
this.Scale = boneNode.Scale;
}
public void Test_Determinant()
{
var m = new Matrix3x3 (1, 2, 1,
1, 1, -1,
2, 1, 1);
Assert.AreEqual (-5, m.Determinant);
}
public void ConstructorValuesAreAccessibleByIndexer()
{
Matrix3x3 matrix3x3;
matrix3x3 = new Matrix3x3();
for (int x = 0; x < matrix3x3.Columns; x++)
{
for (int y = 0; y < matrix3x3.Rows; y++)
{
Assert.Equal(0, matrix3x3[x, y], Epsilon);
}
}
double value = 33.33;
matrix3x3 = new Matrix3x3(value);
for (int x = 0; x < matrix3x3.Columns; x++)
{
for (int y = 0; y < matrix3x3.Rows; y++)
{
Assert.Equal(value, matrix3x3[x, y], Epsilon);
}
}
GenerateFilledMatrixWithValues(out matrix3x3);
for (int y = 0; y < matrix3x3.Rows; y++)
{
for (int x = 0; x < matrix3x3.Columns; x++)
{
Assert.Equal(y * matrix3x3.Columns + x, matrix3x3[x, y], Epsilon);
}
}
}
/// <summary>
/// Set transform matrix
/// </summary>
/// <param name="transformMatrix">transform matrix</param>
public /*internal*/ virtual void SetTransformMatrix(Matrix3x3 transformMatrix)
{
if (transformMatrix != null)
{
CurrentTransformMatrix = transformMatrix;
// check if current transform is transformed
IsTransformed = transformMatrix.IsTransformed;
if (IsTransformed)
{
#region calculate inverted matrix
Matrix3x3 InvertedMatrix = transformMatrix.InvertedMatrix;
if (InvertedMatrix != null)
{
InvertedMatrixSx = InvertedMatrix.Sx;
InvertedMatrixSy = InvertedMatrix.Sy;
InvertedMatrixShy = InvertedMatrix.Shy;
InvertedMatrixShx = InvertedMatrix.Shx;
InvertedMatrixTx = InvertedMatrix.Tx;
InvertedMatrixTy = InvertedMatrix.Ty;
}
else
{
InvertedMatrixSx = 0.0;
InvertedMatrixSy = 0.0;
InvertedMatrixShy = 0.0;
InvertedMatrixShx = 0.0;
InvertedMatrixTx = 0.0;
InvertedMatrixTy = 0.0;
}
#endregion
}
}
}
///<summary>
/// Receive a rotation matrix of type Matrix4
/// Return a Vector3Float containing the rotation around the 3 axis
///</summary>
public static Vecto3Float computeEulerFromMatrixXYZ(Matrix3x3 m)
{
Vecto3Float sol = new Vecto3Float();
if (m.matrix[0, 2] < 1f)
{
if (m.matrix[0, 2] > -1f)
{
sol.X = (float)Math.Atan2(-m.matrix[1, 2], m.matrix[2, 2]);
sol.Y = (float)Math.Asin(-(m.matrix[0, 2]));
sol.Z = (float)Math.Atan2(-m.matrix[0, 1], m.matrix[0, 0]);
}
else
{
sol.X = -(float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]);
sol.Y = -(float)(Math.PI) / 2;
sol.Z = 0f;
}
}
else
{
sol.X = (float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]);
sol.Y = (float)(Math.PI) / 2;
sol.Z = 0f;
}
return sol;
}
public Matrix3x3 GetSymInverse33(Matrix3x3 M)
{
float det = Vector3.Dot(Column1, Vector3.Cross(Column2, Column3));
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = Column1.X, a12 = Column2.X, a13 = Column3.X;
float a22 = Column2.Y, a23 = Column3.Y;
float a33 = Column3.Z;
M.Column1.X = det * (a22 * a33 - a23 * a23);
M.Column1.Y = det * (a13 * a23 - a12 * a33);
M.Column1.Z = det * (a12 * a23 - a13 * a22);
M.Column2.X = M.Column1.Y;
M.Column2.Y = det * (a11 * a33 - a13 * a13);
M.Column2.Z = det * (a13 * a12 - a11 * a23);
M.Column3.X = M.Column1.Z;
M.Column3.Y = M.Column2.Z;
M.Column3.Z = det * (a11 * a22 - a12 * a12);
return M;
}
public NoRotationJointDescriptor(Matrix3x3 anchorOrientationALocal, Matrix3x3 anchorOrientationBLocal, IRigidBody rigidBodyA = null, IRigidBody rigidBodyB = null, object userData = null) : this()
{
AnchorOrientationALocal = anchorOrientationALocal;
AnchorOrientationBLocal = anchorOrientationBLocal;
RigidBodyA = rigidBodyA;
RigidBodyB = rigidBodyB;
UserData = userData;
}
public override void ReadData(GrnBinaryReader reader, int directoryOffset)
{
reader.Seek((int)(this.Offset + directoryOffset), SeekOrigin.Begin);
this.ParentIndex = reader.ReadInt32();
this.Position = reader.ReadVector3D();
this.Rotation = reader.ReadQuaternion();
this.Scale = reader.ReadMatrix3x3();
if (this.GetReadDataLength() != this.GetWriteDataLength()) throw new Exception("bbb");
}
public void ConstantValuesAreCorrect()
{
Matrix3x3 matrix3x3 = new Matrix3x3();
Assert.Equal(3, matrix3x3.Columns);
Assert.Equal(3, matrix3x3.Rows);
Assert.Equal(Matrix3x3.ColumnCount, matrix3x3.Columns);
Assert.Equal(Matrix3x3.RowCount, matrix3x3.Rows);
}
public static Matrix3x3 operator -(Matrix3x3 m1, Matrix3x3 m2)
{
Matrix3x3 kSum = new Matrix3x3();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kSum.m[iRow,iCol] = m1.m[iRow,iCol] - m2.m[iRow,iCol];
}
}
return kSum;
}
public static Matrix3x3 operator *(Matrix3x3 m, float s)
{
Matrix3x3 kProd = new Matrix3x3();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kProd.m[iRow,iCol] = m.m[iRow,iCol] * s;
}
}
return kProd;
}
static void Rotate12(ref SymmetricMatrix3x3 vtav, ref Matrix3x3 v)
{
if (vtav.m12 == 0)
{
return;
}
float c, s;
Schur2.Rot12(ref vtav, out c, out s);
Givens.Rot12_post(ref v, c, s);
}
static void GetSymmetricSvd(SymmetricMatrix3x3 a, out SymmetricMatrix3x3 vtav, out Matrix3x3 v, float tol, int max_sweeps)
{
vtav = a;
v = Matrix3x3.identity;
float delta = tol * vtav.FNorm;
for (int i = 0; i < max_sweeps && vtav.Off > delta; ++i)
{
Rotate01(ref vtav, ref v);
Rotate02(ref vtav, ref v);
Rotate12(ref vtav, ref v);
}
}
public void Write(Matrix3x3 m)
{
this.Write(m.A1);
this.Write(m.A2);
this.Write(m.A3);
this.Write(m.B1);
this.Write(m.B2);
this.Write(m.B3);
this.Write(m.C1);
this.Write(m.C2);
this.Write(m.C3);
}
public static Matrix3x3 operator *(Matrix3x3 m1, Matrix3x3 m2)
{
Matrix3x3 kProd = new Matrix3x3();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kProd.m[iRow,iCol] = m1.m[iRow,0] * m2.m[0,iCol] +
m1.m[iRow,1] * m2.m[1,iCol] +
m1.m[iRow,2] * m2.m[2,iCol];
}
}
return kProd;
}
public static Matrix3x3 transpose(Matrix3x3 toTra)
{
Matrix3x3 result = new Matrix3x3();
for (int i = 0; i < DIMENSION; i++)
{
for (int j = 0; j < DIMENSION; j++)
{
result.matrix[j,i] = toTra.matrix[i,j];
}
}
return result;
}
public void Write(Matrix3x3 value)
{
Write(value.m00);
Write(value.m01);
Write(value.m02);
Write(value.m10);
Write(value.m11);
Write(value.m12);
Write(value.m20);
Write(value.m21);
Write(value.m22);
}
public void Test_Equals()
{
var delta = 0.00001f;
var a = new Matrix3x3 (1, 1, 1,
1, 1, 1,
1, 1, 1);
var b = new Matrix3x3 (1 + delta, 1 + delta, 1 + delta,
1 + delta, 1 + delta, 1 + delta,
1 + delta, 1 + delta, 1 + delta);
Assert.IsTrue (a.Equals (b)); // 誤差許容
Assert.IsFalse (a == b); // 厳密な比較
Assert.IsFalse (a.GetHashCode () == b.GetHashCode ()); // 厳密な比較に基づくハッシュ値
}
public Matrix3x3 GetInverse22(Matrix3x3 M)
{
float a = Column1.X, b = Column2.X, c = Column1.Y, d = Column2.Y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
M.Column1.X = det * d; M.Column2.X = -det * b; M.Column1.Z = 0.0f;
M.Column1.Y = -det * c; M.Column2.Y = det * a; M.Column2.Z = 0.0f;
M.Column3.X = 0.0f; M.Column3.Y = 0.0f; M.Column3.Z = 0.0f;
return M;
}
/// <summary>
/// Erstellt aus der angegebenen Matrix eine Matrix4x4, sofern die Abmessungen stimmen.
/// </summary>
public static new Matrix3x3 FromMatrix(Matrix m)
{
if (m.ColumnCount == 3 && m.RowCount == 3)
{
var sm = new Matrix3x3();
for (int x = 0; x < sm.Size; x++)
for (int y = 0; y < sm.Size; y++)
sm[x, y] = m[x, y]; //Wert kopieren
return sm;
}
else //falsche Abmessungen
throw new ArgumentException("Die angegebene Matrix war nicht in eine Matrix3x3 konvertierbar.");
}
public static void Rot12_post(ref Matrix3x3 m, float c, float s)
{
float m01 = m.m01;
float m02 = m.m02;
float m11 = m.m11;
float m12 = m.m12;
float m21 = m.m21;
float m22 = m.m22;
m.m01 = c * m01 - s * m02;
m.m02 = s * m01 + c * m02;
m.m11 = c * m11 - s * m12;
m.m12 = s * m11 + c * m12;
m.m21 = c * m21 - s * m22;
m.m22 = s * m21 + c * m22;
}
public void Test_Convert_1()
{
var mat = new Matrix3x3 (1, 2, 3,
4, 5, 6,
7, 8, 9);
var m = (float[])mat;
Assert.AreEqual (1, m[0]);
Assert.AreEqual (2, m[1]);
Assert.AreEqual (3, m[2]);
Assert.AreEqual (4, m[3]);
Assert.AreEqual (5, m[4]);
Assert.AreEqual (6, m[5]);
Assert.AreEqual (7, m[6]);
Assert.AreEqual (8, m[7]);
Assert.AreEqual (9, m[8]);
}
public static void Rot01_post(ref Matrix3x3 m, float c, float s)
{
float m00 = m.m00;
float m01 = m.m01;
float m10 = m.m10;
float m11 = m.m11;
float m20 = m.m20;
float m21 = m.m21;
m.m00 = c * m00 - s * m01;
m.m01 = s * m00 + c * m01;
m.m10 = c * m10 - s * m11;
m.m11 = s * m10 + c * m11;
m.m20 = c * m20 - s * m21;
m.m21 = s * m20 + c * m21;
}
public static void Rot02_post(ref Matrix3x3 m, float c, float s)
{
float m00 = m.m00;
float m02 = m.m02;
float m10 = m.m10;
float m12 = m.m12;
float m20 = m.m20;
float m22 = m.m22;
m.m00 = c * m00 - s * m02;
m.m02 = s * m00 + c * m02;
m.m10 = c * m10 - s * m12;
m.m12 = s * m10 + c * m12;
m.m20 = c * m20 - s * m22;
m.m22 = s * m20 + c * m22;
}
public Matrix3x3 leftMul(Matrix3x3 mulby)
{
Matrix3x3 result = new Matrix3x3();
for(int i=0; i<DIMENSION; i++){
for (int j = 0; j < DIMENSION; j++)
{
for (int k = 0; k < DIMENSION; k++)
{
result.matrix[i,j] = mulby.matrix[k,j] * this.matrix[i,k] ;
}
}
}
return result;
}
static void Pseudoinverse(out Matrix3x3 outm, SymmetricMatrix3x3 d, Matrix3x3 v, float tol)
{
float d0 = Pinv(d.m00, tol);
float d1 = Pinv(d.m11, tol);
float d2 = Pinv(d.m22, tol);
outm = new Matrix3x3();
outm.m00 = v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02;
outm.m01 = v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12;
outm.m02 = v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22;
outm.m10 = v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02;
outm.m11 = v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12;
outm.m12 = v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22;
outm.m20 = v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02;
outm.m21 = v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12;
outm.m22 = v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22;
}
public void IndexerGetAndSetValuesCorrectly()
{
Matrix3x3 matrix3x3 = new Matrix3x3();
for (int x = 0; x < matrix3x3.Columns; x++)
{
for (int y = 0; y < matrix3x3.Rows; y++)
{
matrix3x3[x, y] = y * matrix3x3.Columns + x;
}
}
for (int y = 0; y < matrix3x3.Rows; y++)
{
for (int x = 0; x < matrix3x3.Columns; x++)
{
Assert.Equal(y * matrix3x3.Columns + x, matrix3x3[x, y], Epsilon);
}
}
}
public static MinkowskiDiff Support(Entity Object1, Entity Object2, Vector2 axis)
{
Matrix3x3 transform1, transform2;
{
Matrix3x3 rotation1 = Matrix3x3.RotationZ(-Object1.Orientation);
Matrix3x3 translation1 = new Matrix3x3(1, 0, Object1.Position.X, 0, 1, Object1.Position.Y, 0, 0, 1);
transform1 = Matrix3x3.Multiply(translation1, rotation1);
Matrix3x3 rotation2 = Matrix3x3.RotationZ(-Object2.Orientation);
Matrix3x3 translation2 = new Matrix3x3(1, 0, Object2.Position.X, 0, 1, Object2.Position.Y, 0, 0, 1);
transform2 = Matrix3x3.Multiply(translation2, rotation2);
}
float max1 = axis.Dot(Object1.Shape.Vertices[0].Transform(transform1));
Vector2 maxp1 = Object1.Shape.Vertices[0].Transform(transform1);
float max2 = axis.Dot(Object2.Shape.Vertices[0].Transform(transform2));
Vector2 maxp2 = Object2.Shape.Vertices[0].Transform(transform2);
for (int i = 1; i < Object1.Shape.Vertices.Length; i++)
{
float dot = axis.Dot(Object1.Shape.Vertices[i].Transform(transform1));
if (max1 < dot)
{
max1 = dot;
maxp1 = Object1.Shape.Vertices[i].Transform(transform1);
}
}
for (int i = 1; i < Object2.Shape.Vertices.Length; i++)
{
float dot = axis.Dot(Object2.Shape.Vertices[i].Transform(transform2));
if (max2 > dot)
{
max2 = dot;
maxp2 = Object2.Shape.Vertices[i].Transform(transform2);
}
}
return new MinkowskiDiff(maxp1, maxp2);
}
public void TestEquals()
{
Matrix3x3 m1 = new Matrix3x3(1.0f, 2.0f, 3.0f, 0.0f, -5.0f, .5f, .3f, .35f, .025f);
Matrix3x3 m2 = new Matrix3x3(1.0f, 2.0f, 3.0f, 0.0f, -5.0f, .5f, .3f, .35f, .025f);
Matrix3x3 m3 = new Matrix3x3(0.0f, 2.0f, 25.0f, 1.0f, 5.0f, 5.5f, 1.25f, 8.5f, 2.25f);
//Test IEquatable Equals
Assert.IsTrue(m1.Equals(m2), "Test IEquatable equals");
Assert.IsFalse(m1.Equals(m3), "Test IEquatable equals");
//Test object equals override
Assert.IsTrue(m1.Equals((object) m2), "Tests object equals");
Assert.IsFalse(m1.Equals((object) m3), "Tests object equals");
//Test op equals
Assert.IsTrue(m1 == m2, "Testing OpEquals");
Assert.IsFalse(m1 == m3, "Testing OpEquals");
//Test op not equals
Assert.IsTrue(m1 != m3, "Testing OpNotEquals");
Assert.IsFalse(m1 != m2, "Testing OpNotEquals");
}
public void Test_Convert_2()
{
var mat = new Matrix3x3 (1, 2, 3,
4, 5, 6,
7, 8, 9);
var m = (Matrix4x4)mat;
Assert.AreEqual (1, m[0]);
Assert.AreEqual (2, m[1]);
Assert.AreEqual (3, m[2]);
Assert.AreEqual (0, m[3]);
Assert.AreEqual (4, m[4]);
Assert.AreEqual (5, m[5]);
Assert.AreEqual (6, m[6]);
Assert.AreEqual (0, m[7]);
Assert.AreEqual (7, m[8]);
Assert.AreEqual (8, m[9]);
Assert.AreEqual (9, m[10]);
Assert.AreEqual (0, m[11]);
Assert.AreEqual (0, m[12]);
Assert.AreEqual (0, m[13]);
Assert.AreEqual (0, m[14]);
Assert.AreEqual (1, m[15]);
}
/// <summary>
/// Estimate pose of a model from it's projected 2D coordinates.
/// </summary>
///
/// <param name="points">4 2D points of the <see cref="Model">model's</see> projection.</param>
/// <param name="rotation">Gets object's rotation.</param>
/// <param name="translation">Gets object's translation.</param>
///
/// <exception cref="ArgumentException">4 points must be be given for pose estimation.</exception>
///
public void EstimatePose(Point[] points, out Matrix3x3 rotation, out Vector3 translation)
{
if (points.Length != 4)
{
throw new ArgumentException("4 points must be be given for pose estimation.");
}
float Z0 = 0, scale = 1;
Vector3 X0 = new Vector3(points[0].X);
Vector3 Y0 = new Vector3(points[0].Y);
Vector3 XI = new Vector3(points[1].X, points[2].X, points[3].X);
Vector3 YI = new Vector3(points[1].Y, points[2].Y, points[3].Y);
int count = 0;
Vector3 iVector = new Vector3( );
Vector3 jVector = new Vector3( );
Vector3 kVector = new Vector3( );
Vector3 imageXs = new Vector3( );
Vector3 imageYs = new Vector3( );
Vector3 eps = new Vector3(1);
for ( ; count < 100; count++)
{
// calculate new scale orthographic projection (SOP)
imageXs = XI * eps - X0;
imageYs = YI * eps - Y0;
// calculate I and J vectors
iVector = modelPseudoInverse * imageXs;
jVector = modelPseudoInverse * imageYs;
// convert them to unit vectors i and j
float iNorm = iVector.Normalize( );
float jNorm = jVector.Normalize( );
// scale of projection
scale = (iNorm + jNorm) / 2;
// calculate n vector k
kVector = Vector3.Cross(iVector, jVector);
// z-coordinate Z0 of the translation vector
Z0 = focalLength / scale;
// calculate new epsilon values
Vector3 oldEps = eps;
eps = (modelVectors * kVector) / Z0 + 1;
// check if it is time to stop
if ((eps - oldEps).Abs( ).Max < stop_epsilon)
{
break;
}
}
// create rotation matrix
rotation = Matrix3x3.CreateFromRows(iVector, jVector, kVector);
// create translation vector
Vector3 temp = rotation * modelPoints[0];
translation = new Vector3(
points[0].X / scale - temp.X,
points[0].Y / scale - temp.Y,
focalLength / scale);
}
void _SolveEyes(ref Matrix3x3 neckBasis, ref Matrix3x3 headBasis, ref Matrix3x3 headBaseBasis,
ref Quaternion headPrevRotation, ref Quaternion leftEyePrevRotation, ref Quaternion rightEyePrevRotation)
{
if (_headBone != null && _headBone.transformIsAlive)
{
if ((_leftEyeBone != null && _leftEyeBone.transformIsAlive) || (_rightEyeBone != null && _rightEyeBone.transformIsAlive))
{
Vector3 headWorldPosition, neckBoneWorldPosition = _neckBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out headWorldPosition, ref neckBasis, ref _headBone._defaultPosition, ref _neckBone._defaultPosition, ref neckBoneWorldPosition);
Vector3 eyesPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref headBasis, ref _eyesEffector._defaultPosition, ref _headBone._defaultPosition, ref headWorldPosition);
Vector3 eyesDir = _eyesEffector.worldPosition - eyesPosition;
SAFBIKMatMultVecInv(out eyesDir, ref headBaseBasis, ref eyesDir);
SAFBIKVecNormalize(ref eyesDir);
if (_internalValues.resetTransforms && _eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Vector3 tempDir = Vector3.Lerp(new Vector3(0.0f, 0.0f, 1.0f), eyesDir, _eyesEffector.positionWeight);
if (SAFBIKVecNormalize(ref tempDir))
{
eyesDir = tempDir;
}
}
_LimitXY_Square(ref eyesDir,
_internalValues.headIK.eyesLimitYawTheta.sin,
_internalValues.headIK.eyesLimitYawTheta.sin,
_internalValues.headIK.eyesLimitPitchTheta.sin,
_internalValues.headIK.eyesLimitPitchTheta.sin);
eyesDir.x *= _settings.headIK.eyesYawRate;
eyesDir.y *= _settings.headIK.eyesPitchRate;
Vector3 leftEyeDir = eyesDir;
Vector3 rightEyeDir = eyesDir;
if (eyesDir.x >= 0.0f)
{
leftEyeDir.x *= _settings.headIK.eyesYawInnerRate;
rightEyeDir.x *= _settings.headIK.eyesYawOuterRate;
}
else
{
leftEyeDir.x *= _settings.headIK.eyesYawOuterRate;
rightEyeDir.x *= _settings.headIK.eyesYawInnerRate;
}
SAFBIKVecNormalize2(ref leftEyeDir, ref rightEyeDir);
SAFBIKMatMultVec(out leftEyeDir, ref headBaseBasis, ref leftEyeDir);
SAFBIKMatMultVec(out rightEyeDir, ref headBaseBasis, ref rightEyeDir);
Quaternion worldRotation;
Quaternion headBoneWorldRotation = _headBone.worldRotation;
if (_leftEyeBone != null && _leftEyeBone.transformIsAlive)
{
Matrix3x3 leftEyeBaseBasis;
SAFBIKComputeBasisLockZ(out leftEyeBaseBasis, ref headBasis.column0, ref headBasis.column1, ref leftEyeDir);
SAFBIKMatMultGetRot(out worldRotation, ref leftEyeBaseBasis, ref _leftEyeBone._baseToWorldBasis);
if (!_internalValues.resetTransforms && _eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
SAFBIKQuatMultNorm3Inv1(out fromRotation, ref headBoneWorldRotation, ref headPrevRotation, ref leftEyePrevRotation);
_leftEyeBone.worldRotation = Quaternion.Lerp(fromRotation, worldRotation, _eyesEffector.positionWeight);
}
else
{
_leftEyeBone.worldRotation = worldRotation;
}
}
if (_rightEyeBone != null && _rightEyeBone.transformIsAlive)
{
Matrix3x3 rightEyeBaseBasis;
SAFBIKComputeBasisLockZ(out rightEyeBaseBasis, ref headBasis.column0, ref headBasis.column1, ref rightEyeDir);
SAFBIKMatMultGetRot(out worldRotation, ref rightEyeBaseBasis, ref _rightEyeBone._baseToWorldBasis);
if (!_internalValues.resetTransforms && _eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
SAFBIKQuatMultNorm3Inv1(out fromRotation, ref headBoneWorldRotation, ref headPrevRotation, ref rightEyePrevRotation);
_rightEyeBone.worldRotation = Quaternion.Lerp(fromRotation, worldRotation, _eyesEffector.positionWeight);
}
else
{
_rightEyeBone.worldRotation = worldRotation;
}
}
}
}
}
/// <summary>
/// Transforms a set of local extreme points by a linear transform and stores each axis result in the appropriate component of the result vector.
/// Requires that the input variables and output variables are separate.
/// </summary>
/// <param name="x">Local extreme point along the world X axis.</param>
/// <param name="y">Local extreme point along the world Y axis.</param>
/// <param name="z">Local extreme point along the world Z axis.</param>
/// <param name="transform">World transform.</param>
/// <param name="result">Contains the transformed X coordinate of input X, transformed Y coordinate of input Y, and transformed Z coordinate of input Z.</param>
protected void TransformLocalExtremePoints(ref Vector3 x, ref Vector3 y, ref Vector3 z, ref Matrix3x3 transform,
out Vector3 result)
{
result.X = x.X * transform.M11 + x.Y * transform.M21 + x.Z * transform.M31;
result.Y = y.X * transform.M12 + y.Y * transform.M22 + y.Z * transform.M32;
result.Z = z.X * transform.M13 + z.Y * transform.M23 + z.Z * transform.M33;
}
internal void ComputeWorldSpaceData()
{
//Transform local space vectors to world space.
RigidTransform.Transform(ref localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.worldTransform, out worldAttachmentPoint);
Matrix3x3.Transform(ref localDirection, ref wheel.vehicle.Body.orientationMatrix, out worldDirection);
}
public Matrix3x3(Matrix3x3 m)
{
System.Array.Copy(m.m, this.m, 9);
}
protected override void ProcessCandidates(ref QuickList <ContactData> candidates)
{
if (candidates.Count == 0 && parentContactCount == 0 && Mesh.Shape.solidity == MobileMeshSolidity.Solid)
{
//If there's no new contacts on the mesh and it's supposed to be a solid,
//then we must check the convex for containment within the shell.
//We already know that it's not on the shell, meaning that the shape is either
//far enough away outside the shell that there's no contact (and we're done),
//or it's far enough inside the shell that the triangles cannot create contacts.
//To find out which it is, raycast against the shell.
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out orientation);
Ray ray;
Vector3.Subtract(ref convex.worldTransform.Position, ref mesh.worldTransform.Position,
out ray.Position);
Matrix3x3.TransformTranspose(ref ray.Position, ref orientation, out ray.Position);
//Cast from the current position back to the previous position.
Vector3.Subtract(ref lastValidConvexPosition, ref ray.Position, out ray.Direction);
float rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//The object may not have moved enough to normalize properly. If so, choose something arbitrary.
//Try the direction from the center of the object to the convex's position.
ray.Direction = ray.Position;
rayDirectionLength = ray.Direction.LengthSquared();
if (rayDirectionLength < Toolbox.Epsilon)
{
//This is unlikely; just pick something completely arbitrary then.
ray.Direction = Vector3.Up;
rayDirectionLength = 1;
}
}
Vector3.Divide(ref ray.Direction, (float)Math.Sqrt(rayDirectionLength), out ray.Direction);
RayHit hit;
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
ContactData newContact = new ContactData {
Id = 2
};
//Give it a special id so that we know that it came from the inside.
Matrix3x3.Transform(ref ray.Position, ref orientation, out newContact.Position);
Vector3.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position);
newContact.Normal = hit.Normal;
newContact.Normal.Normalize();
float factor;
Vector3.Dot(ref ray.Direction, ref newContact.Normal, out factor);
newContact.PenetrationDepth = -factor * hit.T + convex.Shape.MinimumRadius;
Matrix3x3.Transform(ref newContact.Normal, ref orientation, out newContact.Normal);
newContact.Validate();
//Do not yet create a new contact. Check to see if an 'inner contact' with id == 2 already exists.
bool addContact = true;
for (int i = 0; i < contacts.Count; i++)
{
if (contacts.Elements[i].Id == 2)
{
contacts.Elements[i].Position = newContact.Position;
contacts.Elements[i].Normal = newContact.Normal;
contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth;
supplementData.Elements[i].LocalOffsetA = new Vector3();
supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh.
addContact = false;
break;
}
}
if (addContact && contacts.Count == 0)
{
Add(ref newContact);
}
previousDepth = newContact.PenetrationDepth;
}
else
{
//It's possible that we had a false negative. The previous frame may have been in deep intersection, and this frame just failed to come to the same conclusion.
//If we set the target location to the current location, the object will never escape the mesh. Instead, only do that if two frames agree that we are no longer colliding.
if (previousDepth > 0)
//We're not touching the mesh.
{
lastValidConvexPosition = ray.Position;
}
previousDepth = 0;
}
}
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
float minX, maxX;
float minY, maxY;
float minZ, maxZ;
var right = new Vector3(o.M11, o.M21, o.M31);
var up = new Vector3(o.M12, o.M22, o.M32);
var backward = new Vector3(o.M13, o.M23, o.M33);
Vector3.Dot(ref vertices[0], ref right, out maxX);
minX = maxX;
Vector3.Dot(ref vertices[0], ref up, out maxY);
minY = maxY;
Vector3.Dot(ref vertices[0], ref backward, out maxZ);
minZ = maxZ;
int minXIndex = 0;
int maxXIndex = 0;
int minYIndex = 0;
int maxYIndex = 0;
int minZIndex = 0;
int maxZIndex = 0;
for (int i = 1; i < vertices.Length; ++i)
{
float dot;
Vector3.Dot(ref vertices[i], ref right, out dot);
if (dot < minX)
{
minX = dot;
minXIndex = i;
}
else if (dot > maxX)
{
maxX = dot;
maxXIndex = i;
}
Vector3.Dot(ref vertices[i], ref up, out dot);
if (dot < minY)
{
minY = dot;
minYIndex = i;
}
else if (dot > maxY)
{
maxY = dot;
maxYIndex = i;
}
Vector3.Dot(ref vertices[i], ref backward, out dot);
if (dot < minZ)
{
minZ = dot;
minZIndex = i;
}
else if (dot > maxZ)
{
maxZ = dot;
maxZIndex = i;
}
}
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref vertices[maxXIndex], ref vertices[maxYIndex], ref vertices[maxZIndex], ref o, out positive);
TransformLocalExtremePoints(ref vertices[minXIndex], ref vertices[minYIndex], ref vertices[minZIndex], ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform point into world space.
Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r);
Vector3.Add(ref r, ref entity.position, out worldPoint);
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism)
{
Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error);
float separationDistance = error.Length();
if (separationDistance > Toolbox.BigEpsilon)
{
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
//The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity.
//The constant correction velocity could overshoot the destination, so clamp it.
float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) +
separationDistance * errorReduction;
Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity);
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > settings.servo.maxCorrectiveVelocitySquared)
{
float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
else
{
//Wouldn't want to use a bias from an earlier frame.
biasVelocity = new Vector3();
}
}
else
{
usedSoftness = settings.velocityMotor.softness * updateRate;
biasVelocity = settings.velocityMotor.goalVelocity;
error = Vector3.Zero;
}
//Compute the maximum force that can be applied this frame.
ComputeMaxForces(settings.maximumForce, dt);
//COMPUTE EFFECTIVE MASS MATRIX
//Transforms a change in velocity to a change in momentum when multiplied.
Matrix3x3 linearComponent;
Matrix3x3.CreateScale(entity.inverseMass, out linearComponent);
Matrix3x3 rACrossProduct;
Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
}
/// <summary>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//Find all hits. Use the count to determine the ray started inside or outside.
//If it starts inside and we're in 'solid' mode, then return the ray start.
//The raycast must be of infinite length at first. This allows it to determine
//if it is inside or outside.
if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit))
{
//It was inside!
rayHit = new RayHit()
{
Location = ray.Position, Normal = Vector3.Zero, T = 0
};
return(true);
}
else
{
if (rayHit.T < maximumLength)
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
}
else
{
//The hit was too far away, or there was no hit (in which case T would be float.MaxValue).
return(false);
}
return(true);
}
}
else
{
//Just do a normal raycast since the object isn't solid.
TriangleSidedness sidedness;
switch (Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
default:
sidedness = TriangleSidedness.DoubleSided;
break;
}
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
}
rayHit = new RayHit();
return(false);
}
/// <summary>
/// Sets the collision information of the entity to another collidable.
/// </summary>
/// <param name="newCollisionInformation">New collidable to use.</param>
/// <param name="newMass">New mass to use for the entity.</param>
/// <param name="newInertia">New inertia tensor to use for the entity.</param>
public void SetCollisionInformation(EntityCollidable newCollisionInformation, float newMass, Matrix3x3 newInertia)
{
//Temporarily remove the object from the space.
//The reset process will update any systems that need to be updated.
//This is not thread safe, but this operation should not be performed mid-frame anyway.
Space space = Space;
if (space != null)
{
Space.Remove(this);
}
CollisionInformation.Entity = null;
Initialize(newCollisionInformation, newMass, newInertia);
if (space != null)
{
space.Add(this);
}
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//Transform local stuff into world space
Vector3 worldRestrictedAxis1, worldRestrictedAxis2;
Quaternion.Transform(ref localRestrictedAxis1, ref ConnectionA.Orientation, out worldRestrictedAxis1);
Quaternion.Transform(ref localRestrictedAxis2, ref ConnectionA.Orientation, out worldRestrictedAxis2);
Vector3 worldLineAnchor;
Quaternion.Transform(ref LocalLineAnchor, ref ConnectionA.Orientation, out worldLineAnchor);
Vector3.Add(ref worldLineAnchor, ref ConnectionA.Position, out worldLineAnchor);
Vector3 lineDirection;
Quaternion.Transform(ref localLineDirection, ref ConnectionA.Orientation, out lineDirection);
Vector3 rB;
Quaternion.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out rB);
Vector3 worldPoint;
Vector3.Add(ref rB, ref ConnectionB.Position, out worldPoint);
//Find the point on the line closest to the world point.
Vector3 offset;
Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
float distanceAlongAxis;
Vector3.Dot(ref offset, ref lineDirection, out distanceAlongAxis);
Vector3 worldNearPoint;
Vector3.Multiply(ref lineDirection, distanceAlongAxis, out offset);
Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
Vector3 rA;
Vector3.Subtract(ref worldNearPoint, ref ConnectionA.Position, out rA);
//Error
Vector3 error3D;
Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D);
Vector2 error;
Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);
velocityBias.X = errorCorrectionFactor * error.X;
velocityBias.Y = errorCorrectionFactor * error.Y;
//Set up the jacobians
Vector3 angularA1, angularA2, angularB1, angularB2;
Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);
//Put all the 1x3 jacobians into a 3x3 matrix representation.
linearJacobianA = new Matrix3x3
{
M11 = worldRestrictedAxis1.X,
M12 = worldRestrictedAxis1.Y,
M13 = worldRestrictedAxis1.Z,
M21 = worldRestrictedAxis2.X,
M22 = worldRestrictedAxis2.Y,
M23 = worldRestrictedAxis2.Z
};
Matrix3x3.Negate(ref linearJacobianA, out linearJacobianB);
angularJacobianA = new Matrix3x3
{
M11 = angularA1.X,
M12 = angularA1.Y,
M13 = angularA1.Z,
M21 = angularA2.X,
M22 = angularA2.Y,
M23 = angularA2.Z
};
angularJacobianB = new Matrix3x3
{
M11 = angularB1.X,
M12 = angularB1.Y,
M13 = angularB1.Z,
M21 = angularB2.X,
M22 = angularB2.Y,
M23 = angularB2.Z
};
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform the axes into world space.
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism)
{
float y, x;
Vector3 yAxis;
Vector3.Cross(ref basis.primaryAxis, ref basis.xAxis, out yAxis);
Vector3.Dot(ref worldTestAxis, ref yAxis, out y);
Vector3.Dot(ref worldTestAxis, ref basis.xAxis, out x);
var angle = (float)Math.Atan2(y, x);
//****** VELOCITY BIAS ******//
//Compute the correction velocity.
error = GetDistanceFromGoal(angle);
float absErrorOverDt = Math.Abs(error * updateRate);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction;
biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity);
}
else
{
biasVelocity = settings.velocityMotor.goalVelocity;
usedSoftness = settings.velocityMotor.softness * updateRate;
error = 0;
}
//Compute the jacobians
jacobianA = basis.primaryAxis;
jacobianB.X = -jacobianA.X;
jacobianB.Y = -jacobianA.Y;
jacobianB.Z = -jacobianA.Z;
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
}
else
{
entryA = 0;
}
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
{
entryB = 0;
}
//Compute the inverse mass matrix
velocityToImpulse = 1 / (usedSoftness + entryA + entryB);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}
void IForceUpdateable.UpdateForForces(float dt)
{
//Apply gravity.
if (hasPersonalGravity)
{
Vector3 gravityDt;
Vector3.Multiply(ref personalGravity, dt, out gravityDt);
Vector3.Add(ref gravityDt, ref linearVelocity, out linearVelocity);
}
else
{
Vector3.Add(ref forceUpdater.gravityDt, ref linearVelocity, out linearVelocity);
}
//Boost damping at very low velocities. This is a strong stabilizer; removes a ton of energy from the system.
if (activityInformation.DeactivationManager.useStabilization && activityInformation.allowStabilization &&
(activityInformation.isSlowing || activityInformation.velocityTimeBelowLimit > activityInformation.DeactivationManager.lowVelocityTimeMinimum))
{
float energy = linearVelocity.LengthSquared() + angularVelocity.LengthSquared();
if (energy < activityInformation.DeactivationManager.velocityLowerLimitSquared)
{
float boost = 1 - (float)(Math.Sqrt(energy) / (2f * activityInformation.DeactivationManager.velocityLowerLimit));
ModifyAngularDamping(boost);
ModifyLinearDamping(boost);
}
}
//Damping
float linear = LinearDamping + linearDampingBoost;
if (linear > 0)
{
Vector3.Multiply(ref linearVelocity, (float)Math.Pow(MathHelper.Clamp(1 - linear, 0, 1), dt), out linearVelocity);
}
//When applying angular damping, the momentum or velocity is damped depending on the conservation setting.
float angular = AngularDamping + angularDampingBoost;
if (angular > 0)
{
#if CONSERVE
Vector3.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum);
#else
Vector3.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity);
#endif
}
linearDampingBoost = 0;
angularDampingBoost = 0;
//Update world inertia tensors.
Matrix3x3 multiplied;
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse);
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor);
#if CONSERVE
//Update angular velocity.
//Note that this doesn't play nice with singular inertia tensors.
//Locked tensors result in zero angular velocity.
Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity);
MathChecker.Validate(angularMomentum);
#endif
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
}
//Expand the convex's bounding box to include the mobile mesh's movement.
protected internal override int FindOverlappingTriangles(float dt)
{
BoundingBox boundingBox;
AffineTransform transform =
new AffineTransform(mesh.worldTransform.Orientation, mesh.worldTransform.Position);
convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref transform, out boundingBox);
Vector3 transformedVelocity;
//Compute the relative velocity with respect to the mesh. The mesh's bounding tree is NOT expanded with velocity,
//so whatever motion there is between the two objects needs to be included in the convex's bounding box.
if (convex.entity != null)
{
transformedVelocity = convex.entity.linearVelocity;
}
else
{
transformedVelocity = new Vector3();
}
if (mesh.entity != null)
{
Vector3.Subtract(ref transformedVelocity, ref mesh.entity.linearVelocity, out transformedVelocity);
}
//The linear transform is known to be orientation only, so using the transpose is allowed.
Matrix3x3.TransformTranspose(ref transformedVelocity, ref transform.LinearTransform,
out transformedVelocity);
Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity);
if (transformedVelocity.X > 0)
{
boundingBox.Max.X += transformedVelocity.X;
}
else
{
boundingBox.Min.X += transformedVelocity.X;
}
if (transformedVelocity.Y > 0)
{
boundingBox.Max.Y += transformedVelocity.Y;
}
else
{
boundingBox.Min.Y += transformedVelocity.Y;
}
if (transformedVelocity.Z > 0)
{
boundingBox.Max.Z += transformedVelocity.Z;
}
else
{
boundingBox.Min.Z += transformedVelocity.Z;
}
mesh.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, overlappedTriangles);
return(overlappedTriangles.Count);
}
/// <summary>
/// Recenters the triangle data and computes the volume distribution.
/// </summary>
/// <param name="data">Mesh data to analyze.</param>
/// <returns>Computed center, volume, and volume distribution.</returns>
private ShapeDistributionInformation ComputeVolumeDistribution(TransformableMeshData data)
{
//Compute the surface vertices of the shape.
ShapeDistributionInformation shapeInformation;
if (solidity == MobileMeshSolidity.Solid)
{
//The following inertia tensor calculation assumes a closed mesh.
var transformedVertices = CommonResources.GetVectorList();
if (transformedVertices.Capacity < data.vertices.Length)
{
transformedVertices.Capacity = data.vertices.Length;
}
transformedVertices.Count = data.vertices.Length;
for (int i = 0; i < data.vertices.Length; ++i)
{
data.GetVertexPosition(i, out transformedVertices.Elements[i]);
}
InertiaHelper.ComputeShapeDistribution(transformedVertices, data.indices, out shapeInformation.Center, out shapeInformation.Volume, out shapeInformation.VolumeDistribution);
CommonResources.GiveBack(transformedVertices);
if (shapeInformation.Volume > 0)
{
return(shapeInformation);
}
throw new ArgumentException("A solid mesh must have volume.");
}
shapeInformation.Center = new Vector3();
shapeInformation.VolumeDistribution = new Matrix3x3();
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
//Compute the center contribution.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
float perVertexWeight = weight * (1f / 3f);
shapeInformation.Center += perVertexWeight * (vA + vB + vC);
//Compute the inertia contribution of this triangle.
//Approximate it using pointmasses positioned at the triangle vertices.
//(There exists a direct solution, but this approximation will do plenty fine.)
Matrix3x3 aContribution, bContribution, cContribution;
InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vA, out aContribution);
InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vB, out bContribution);
InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vC, out cContribution);
Matrix3x3.Add(ref aContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution);
Matrix3x3.Add(ref bContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution);
Matrix3x3.Add(ref cContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution);
}
shapeInformation.Center /= totalWeight;
//The extra factor of 2 is used because the cross product length was twice the actual area.
Matrix3x3.Multiply(ref shapeInformation.VolumeDistribution, 1 / (2 * totalWeight), out shapeInformation.VolumeDistribution);
//Move the inertia tensor into position according to the center.
Matrix3x3 additionalInertia;
InertiaHelper.GetPointContribution(0.5f, ref Toolbox.ZeroVector, ref shapeInformation.Center, out additionalInertia);
Matrix3x3.Subtract(ref shapeInformation.VolumeDistribution, ref additionalInertia, out shapeInformation.VolumeDistribution);
shapeInformation.Volume = 0;
return(shapeInformation);
}
/// <summary> /// Returns <see cref="RectangleBaseline" /> that is this <see cref="RectangleBaseline" /> transformed by given /// <see cref="Matrix3x3" />. /// </summary> /// <param name="transform">Transformation matrix used to transform rectangle.</param> /// <returns><see cref="RectangleBaseline" /> transformed by given matrix.</returns> public new RectangleBaseline Transform(Matrix3x3 transform) => new RectangleBaseline(base.Transform(transform));
internal void ApplyAngularImpulse(ref System.Numerics.Vector3 impulse)
{
System.Numerics.Vector3 velocityChange;
Matrix3x3.Transform(ref impulse, ref inertiaTensorInverse, out velocityChange);
Vector3Ex.Add(ref velocityChange, ref angularVelocity, out angularVelocity);
}
/// <summary>
/// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation.
/// </summary>
/// <param name="splitPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param>
/// <param name="a">Original compound to be split. Children in this compound will be removed and added to the other compound.</param>
/// <param name="b">Compound to receive children removed from the original compound.</param>
/// <param name="distributionInfoA">Volume, volume distribution, and center information about the new form of the original compound collidable.</param>
/// <param name="distributionInfoB">Volume, volume distribution, and center information about the new compound collidable.</param>
/// <param name="weightA">Total weight associated with the new form of the original compound collidable.</param>
/// <param name="weightB">Total weight associated with the new compound collidable.</param>
/// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns>
public static bool SplitCompound(Func <CompoundChild, bool> splitPredicate,
CompoundCollidable a, CompoundCollidable b,
out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB,
out float weightA, out float weightB)
{
bool splitOccurred = false;
for (int i = a.children.Count - 1; i >= 0; i--)
{
//The shape doesn't change during this process. The entity could, though.
//All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
CompoundChild child = a.children.Elements[i];
if (splitPredicate(child))
{
splitOccurred = true;
a.children.FastRemoveAt(i);
b.children.Add(child);
//The child event handler must be unhooked from the old compound and given to the new one.
child.CollisionInformation.events.Parent = b.Events;
}
}
if (!splitOccurred)
{
//No split occurred, so we cannot proceed.
distributionInfoA = new ShapeDistributionInformation();
distributionInfoB = new ShapeDistributionInformation();
weightA = 0;
weightB = 0;
return(false);
}
//Compute the contributions from the original shape to the new form of the original collidable.
distributionInfoA = new ShapeDistributionInformation();
weightA = 0;
distributionInfoB = new ShapeDistributionInformation();
weightB = 0;
for (int i = a.children.Count - 1; i >= 0; i--)
{
CompoundChild child = a.children.Elements[i];
CompoundShapeEntry entry = child.Entry;
Vector3 weightedCenter;
Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out weightedCenter);
Vector3.Add(ref weightedCenter, ref distributionInfoA.Center, out distributionInfoA.Center);
distributionInfoA.Volume += entry.Shape.Volume;
weightA += entry.Weight;
}
for (int i = b.children.Count - 1; i >= 0; i--)
{
CompoundChild child = b.children.Elements[i];
CompoundShapeEntry entry = child.Entry;
Vector3 weightedCenter;
Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out weightedCenter);
Vector3.Add(ref weightedCenter, ref distributionInfoB.Center, out distributionInfoB.Center);
distributionInfoB.Volume += entry.Shape.Volume;
weightB += entry.Weight;
}
//Average the center out.
if (weightA > 0)
{
Vector3.Divide(ref distributionInfoA.Center, weightA, out distributionInfoA.Center);
}
if (weightB > 0)
{
Vector3.Divide(ref distributionInfoB.Center, weightB, out distributionInfoB.Center);
}
//Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
//That is not technically the center of the new collidable- distributionInfoA.Center is.
//Offset the child collidables by -distributionInfoA.Center using their local offset.
Vector3 offsetA;
Vector3.Negate(ref distributionInfoA.Center, out offsetA);
Vector3 offsetB;
Vector3.Negate(ref distributionInfoB.Center, out offsetB);
//Compute the unscaled inertia tensor.
for (int i = a.children.Count - 1; i >= 0; i--)
{
CompoundChild child = a.children.Elements[i];
CompoundShapeEntry entry = child.Entry;
Vector3 transformedOffset;
Quaternion conjugate;
Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
Quaternion.Transform(ref offsetA, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
Matrix3x3 contribution;
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoA.Center,
ref entry.Shape.volumeDistribution, entry.Weight, out contribution);
Matrix3x3.Add(ref contribution, ref distributionInfoA.VolumeDistribution,
out distributionInfoA.VolumeDistribution);
}
for (int i = b.children.Count - 1; i >= 0; i--)
{
CompoundChild child = b.children.Elements[i];
CompoundShapeEntry entry = child.Entry;
Vector3 transformedOffset;
Quaternion conjugate;
Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
Quaternion.Transform(ref offsetB, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
Matrix3x3 contribution;
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoB.Center,
ref entry.Shape.volumeDistribution, entry.Weight, out contribution);
Matrix3x3.Add(ref contribution, ref distributionInfoB.VolumeDistribution,
out distributionInfoB.VolumeDistribution);
}
//Normalize the volume distribution.
Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, 1 / weightA,
out distributionInfoA.VolumeDistribution);
Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, 1 / weightB,
out distributionInfoB.VolumeDistribution);
//Update the hierarchies of the compounds.
//TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
a.hierarchy.Tree.Reconstruct(a.children);
b.hierarchy.Tree.Reconstruct(b.children);
return(true);
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
isTryingToMove = movementDirection3d.LengthSquared() > 0;
maxForceDt = MaximumForce * dt;
maxAccelerationForceDt = MaximumAccelerationForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
Vector3 downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
Vector3 velocityDirection;
Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
Vector3 lineStart = movementDirection3d;
Vector3 lineEnd;
Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
Vector3 toRemove;
Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
Vector3 supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new Vector3();
angularJacobianB2 = new Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirection3d;
linearJacobianA2 = Vector3.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = isTryingToMove ? TargetSpeed : 0;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
Vector3 intermediate;
inverseMass = characterBody.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2x2.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(characterBody.Mass, out massMatrix);
}
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
{
var distanceToBottomOfCharacter = supportFinder.BottomDistance;
if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
{
timeSinceTransition += dt;
}
if (timeSinceTransition >= timeUntilPositionAnchor)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
Vector3 targetPosition;
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
Vector3 error;
Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
{
Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//Sample the local directions from the matrix, implicitly transposed.
var rightDirection = new Vector3(o.M11, o.M21, o.M31);
var upDirection = new Vector3(o.M12, o.M22, o.M32);
var backDirection = new Vector3(o.M13, o.M23, o.M33);
int right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0;
float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue;
for (int i = 0; i < hullVertices.Count; i++)
{
float dotX, dotY, dotZ;
Vector3.Dot(ref rightDirection, ref hullVertices.Elements[i], out dotX);
Vector3.Dot(ref upDirection, ref hullVertices.Elements[i], out dotY);
Vector3.Dot(ref backDirection, ref hullVertices.Elements[i], out dotZ);
if (dotX < minX)
{
minX = dotX;
left = i;
}
if (dotX > maxX)
{
maxX = dotX;
right = i;
}
if (dotY < minY)
{
minY = dotY;
down = i;
}
if (dotY > maxY)
{
maxY = dotY;
up = i;
}
if (dotZ < minZ)
{
minZ = dotZ;
forward = i;
}
if (dotZ > maxZ)
{
maxZ = dotZ;
backward = i;
}
}
//Incorporate the collision margin.
Vector3.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection);
Vector3.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection);
Vector3.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection);
var rightElement = hullVertices.Elements[right];
var leftElement = hullVertices.Elements[left];
var upElement = hullVertices.Elements[up];
var downElement = hullVertices.Elements[down];
var backwardElement = hullVertices.Elements[backward];
var forwardElement = hullVertices.Elements[forward];
Vector3.Add(ref rightElement, ref rightDirection, out rightElement);
Vector3.Subtract(ref leftElement, ref rightDirection, out leftElement);
Vector3.Add(ref upElement, ref upDirection, out upElement);
Vector3.Subtract(ref downElement, ref upDirection, out downElement);
Vector3.Add(ref backwardElement, ref backDirection, out backwardElement);
Vector3.Subtract(ref forwardElement, ref backDirection, out forwardElement);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
TransformLocalExtremePoints(ref rightElement, ref upElement, ref backwardElement, ref o, out boundingBox.Max);
TransformLocalExtremePoints(ref leftElement, ref downElement, ref forwardElement, ref o, out boundingBox.Min);
}
/// <summary>
/// Removes a child from a compound collidable.
/// </summary>
/// <param name="compound">Compound collidable to remove a child from.</param>
/// <param name="removalPredicate">Callback which analyzes a child and determines if it should be removed from the compound.</param>
/// <param name="childContributions">Distribution contributions from all shapes in the compound shape. This can include shapes which are not represented in the compound.</param>
/// <param name="distributionInfo">Distribution information of the new compound.</param>
/// <param name="weight">Total weight of the new compound.</param>
/// <param name="removedWeight">Weight removed from the compound.</param>
/// <param name="removedCenter">Center of the chunk removed from the compound.</param>
/// <returns>Whether or not any removal took place.</returns>
public static bool RemoveChildFromCompound(CompoundCollidable compound, Func <CompoundChild, bool> removalPredicate, IList <ShapeDistributionInformation> childContributions,
out ShapeDistributionInformation distributionInfo, out float weight, out float removedWeight, out Vector3 removedCenter)
{
bool removalOccurred = false;
removedWeight = 0;
removedCenter = new Vector3();
for (int i = compound.children.Count - 1; i >= 0; i--)
{
//The shape doesn't change during this process. The entity could, though.
//All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
var child = compound.children.Elements[i];
if (removalPredicate(child))
{
removalOccurred = true;
var entry = child.Entry;
removedWeight += entry.Weight;
Vector3 toAdd;
Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd);
Vector3.Add(ref removedCenter, ref toAdd, out removedCenter);
//The child event handler must be unhooked from the compound.
child.CollisionInformation.events.Parent = null;
compound.children.FastRemoveAt(i);
}
}
if (!removalOccurred)
{
//No removal occurred, so we cannot proceed.
distributionInfo = new ShapeDistributionInformation();
weight = 0;
return(false);
}
if (removedWeight > 0)
{
Vector3.Divide(ref removedCenter, removedWeight, out removedCenter);
}
//Compute the contributions from the original shape to the new form of the original collidable.
distributionInfo = new ShapeDistributionInformation();
weight = 0;
for (int i = compound.children.Count - 1; i >= 0; i--)
{
var child = compound.children.Elements[i];
var entry = child.Entry;
var contribution = childContributions[child.shapeIndex];
Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center);
Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center);
Vector3.Add(ref contribution.Center, ref distributionInfo.Center, out distributionInfo.Center);
distributionInfo.Volume += contribution.Volume;
weight += entry.Weight;
}
//Average the center out.
Vector3.Divide(ref distributionInfo.Center, weight, out distributionInfo.Center);
//Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
//That is not technically the center of the new collidable- distributionInfo.Center is.
//Offset the child collidables by -distributionInfo.Center using their local offset.
Vector3 offset;
Vector3.Negate(ref distributionInfo.Center, out offset);
//Compute the unscaled inertia tensor.
for (int i = compound.children.Count - 1; i >= 0; i--)
{
var child = compound.children.Elements[i];
var entry = child.Entry;
Vector3 transformedOffset;
Quaternion conjugate;
Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
Vector3.Transform(ref offset, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
var contribution = childContributions[child.shapeIndex];
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfo.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution);
//Vector3.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position);
Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfo.VolumeDistribution, out distributionInfo.VolumeDistribution);
}
//Normalize the volume distribution.
Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, 1 / weight, out distributionInfo.VolumeDistribution);
//Update the hierarchies of the compounds.
//TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
compound.hierarchy.Tree.Reconstruct(compound.children);
return(true);
}
public static string FormatForMathematica(this Matrix3x3 m)
{
return("{" + FormatForMathematica(m.Row1) + ", " + FormatForMathematica(m.Row2) + ", " + FormatForMathematica(m.Row3) + "}");
}
void _Solve(FullBodyIK fullBodyIK)
{
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Matrix3x3 parentBasis;
SAFBIKMatSetRotMultInv1(out parentBasis, ref parentWorldRotation, ref _neckBone.parentBone._defaultRotation);
Matrix3x3 parentBaseBasis;
SAFBIKMatMult(out parentBaseBasis, ref parentBasis, ref _internalValues.defaultRootBasis);
Quaternion parentBaseRotation;
SAFBIKQuatMult(out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation);
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
Quaternion neckBonePrevRotation = Quaternion.identity;
Quaternion headBonePrevRotation = Quaternion.identity;
Quaternion leftEyeBonePrevRotation = Quaternion.identity;
Quaternion rightEyeBonePrevRotation = Quaternion.identity;
if (!_internalValues.resetTransforms)
{
neckBonePrevRotation = _neckBone.worldRotation;
headBonePrevRotation = _headBone.worldRotation;
if (_leftEyeBone != null && _leftEyeBone.transformIsAlive)
{
leftEyeBonePrevRotation = _leftEyeBone.worldRotation;
}
if (_rightEyeBone != null && _rightEyeBone.transformIsAlive)
{
rightEyeBonePrevRotation = _rightEyeBone.worldRotation;
}
}
// for Neck
if (headPositionWeight > IKEpsilon)
{
Matrix3x3 neckBoneBasis;
SAFBIKMatMult(out neckBoneBasis, ref parentBasis, ref _neckBone._localAxisBasis);
Vector3 yDir = _headEffector.worldPosition - _neckBone.worldPosition; // Not use _hidden_worldPosition
if (SAFBIKVecNormalize(ref yDir))
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref neckBoneBasis, ref yDir);
if (_LimitXZ_Square(ref localDir,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitPitchDownTheta.sin,
_internalValues.headIK.neckLimitPitchUpTheta.sin))
{
SAFBIKMatMultVec(out yDir, ref neckBoneBasis, ref localDir);
}
Vector3 xDir = parentBaseBasis.column0;
Vector3 zDir = parentBaseBasis.column2;
if (SAFBIKComputeBasisLockY(out neckBoneBasis, ref xDir, ref yDir, ref zDir))
{
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref neckBoneBasis, ref _neckBone._boneToWorldBasis);
if (headPositionWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult(out fromRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
}
else
{
fromRotation = neckBonePrevRotation; // This is able to use _headBone.worldRotation directly.
}
_neckBone.worldRotation = Quaternion.Lerp(fromRotation, worldRotation, headPositionWeight);
}
else
{
_neckBone.worldRotation = worldRotation;
}
}
}
}
else if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
_neckBone.worldRotation = tempRotation;
}
// for Head / Eyes
if (eyesPositionWeight <= IKEpsilon)
{
float headRotationWeight = _headEffector.rotationEnabled ? _headEffector.rotationWeight : 0.0f;
if (headRotationWeight > IKEpsilon)
{
Quaternion headEffectorWorldRotation = _headEffector.worldRotation;
Quaternion toRotation;
SAFBIKQuatMult(out toRotation, ref headEffectorWorldRotation, ref _headEffectorToWorldRotation);
if (headRotationWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult3(out fromRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation);
}
else
{
// Not use _headBone.worldRotation.
SAFBIKQuatMultNorm3Inv1(out fromRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation);
}
_headBone.worldRotation = Quaternion.Lerp(fromRotation, toRotation, headRotationWeight);
}
else
{
_headBone.worldRotation = toRotation;
}
}
else
{
if (_internalValues.resetTransforms)
{
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headBoneWorldRotation;
SAFBIKQuatMult3(out headBoneWorldRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation);
_headBone.worldRotation = headBoneWorldRotation;
}
}
_HeadRotationLimit();
if (_internalValues.resetTransforms)
{
if (_isEnabledCustomEyes)
{
fullBodyIK._ResetCustomEyes();
}
else
{
_ResetEyes();
}
}
return;
}
{
Vector3 eyesPosition, parentBoneWorldPosition = _neckBone.parentBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref parentBasis, ref _eyesEffector._defaultPosition, ref _neckBone.parentBone._defaultPosition, ref parentBoneWorldPosition);
// Note: Not use _eyesEffector._hidden_worldPosition
Vector3 eyesDir = _eyesEffector.worldPosition - eyesPosition; // Memo: Not normalize yet.
Matrix3x3 neckBaseBasis = parentBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref parentBaseBasis, ref eyesDir);
localDir.y *= _settings.headIK.eyesToNeckPitchRate;
SAFBIKVecNormalize(ref localDir);
if (_ComputeEyesRange(ref localDir, _internalValues.headIK.eyesTraceTheta.cos))
{
if (localDir.y < -_internalValues.headIK.neckLimitPitchDownTheta.sin)
{
localDir.y = -_internalValues.headIK.neckLimitPitchDownTheta.sin;
}
else if (localDir.y > _internalValues.headIK.neckLimitPitchUpTheta.sin)
{
localDir.y = _internalValues.headIK.neckLimitPitchUpTheta.sin;
}
localDir.x = 0.0f;
localDir.z = SAFBIKSqrt(1.0f - localDir.y * localDir.y);
}
SAFBIKMatMultVec(out eyesDir, ref parentBaseBasis, ref localDir);
{
Vector3 xDir = parentBaseBasis.column0;
Vector3 yDir = parentBaseBasis.column1;
Vector3 zDir = eyesDir;
if (!SAFBIKComputeBasisLockZ(out neckBaseBasis, ref xDir, ref yDir, ref zDir))
{
neckBaseBasis = parentBaseBasis; // Failsafe.
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref neckBaseBasis, ref _neckBone._baseToWorldBasis);
if (_eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion neckWorldRotation = Quaternion.Lerp(_neckBone.worldRotation, worldRotation, _eyesEffector.positionWeight); // This is able to use _neckBone.worldRotation directly.
_neckBone.worldRotation = neckWorldRotation;
SAFBIKMatSetRotMult(out neckBaseBasis, ref neckWorldRotation, ref _neckBone._worldToBaseRotation);
}
else
{
_neckBone.worldRotation = worldRotation;
}
}
Matrix3x3 neckBasis;
SAFBIKMatMult(out neckBasis, ref neckBaseBasis, ref _internalValues.defaultRootBasisInv);
Vector3 neckBoneWorldPosition = _neckBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref neckBasis, ref _eyesEffector._defaultPosition, ref _neckBone._defaultPosition, ref neckBoneWorldPosition);
// Note: Not use _eyesEffector._hidden_worldPosition
eyesDir = _eyesEffector.worldPosition - eyesPosition;
Matrix3x3 headBaseBasis = neckBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref neckBaseBasis, ref eyesDir);
localDir.x *= _settings.headIK.eyesToHeadYawRate;
localDir.y *= _settings.headIK.eyesToHeadPitchRate;
SAFBIKVecNormalize(ref localDir);
if (_ComputeEyesRange(ref localDir, _internalValues.headIK.eyesTraceTheta.cos))
{
// Note: Not use _LimitXY() for Stability
_LimitXY_Square(ref localDir,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitPitchDownTheta.sin,
_internalValues.headIK.headLimitPitchUpTheta.sin);
}
SAFBIKMatMultVec(out eyesDir, ref neckBaseBasis, ref localDir);
{
Vector3 xDir = neckBaseBasis.column0;
Vector3 yDir = neckBaseBasis.column1;
Vector3 zDir = eyesDir;
if (!SAFBIKComputeBasisLockZ(out headBaseBasis, ref xDir, ref yDir, ref zDir))
{
headBaseBasis = neckBaseBasis;
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref headBaseBasis, ref _headBone._baseToWorldBasis);
if (_eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headFromWorldRotation;
SAFBIKQuatMultNorm3Inv1(out headFromWorldRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation);
Quaternion headWorldRotation = Quaternion.Lerp(headFromWorldRotation, worldRotation, _eyesEffector.positionWeight);
_headBone.worldRotation = headWorldRotation;
SAFBIKMatSetRotMult(out headBaseBasis, ref headWorldRotation, ref _headBone._worldToBaseRotation);
}
else
{
_headBone.worldRotation = worldRotation;
}
}
Matrix3x3 headBasis;
SAFBIKMatMult(out headBasis, ref headBaseBasis, ref _internalValues.defaultRootBasisInv);
if (_isEnabledCustomEyes)
{
fullBodyIK._SolveCustomEyes(ref neckBasis, ref headBasis, ref headBaseBasis);
}
else
{
_SolveEyes(ref neckBasis, ref headBasis, ref headBaseBasis, ref headBonePrevRotation, ref leftEyeBonePrevRotation, ref rightEyeBonePrevRotation);
}
}
}
///<summary>
/// Constructs a new morphable entity.
///</summary>
///<param name="collisionInformation">Collidable to use with the entity.</param>
///<param name="mass">Mass of the entity.</param>
/// <param name="inertiaTensor">Inertia tensor of the entity.</param>
public MorphableEntity(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor)
: base(collisionInformation, mass, inertiaTensor)
{
}
/// <summary>
/// Do any necessary computations to prepare the constraint for this frame.
/// </summary>
/// <param name="dt">Simulation step length.</param>
public override void Update(float dt)
{
basisA.rotationMatrix = connectionA.orientationMatrix;
basisB.rotationMatrix = connectionB.orientationMatrix;
basisA.ComputeWorldSpaceAxes();
basisB.ComputeWorldSpaceAxes();
if (settings.mode == MotorMode.Servomechanism)
{
Quaternion rotation;
Quaternion.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation);
//Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
Vector3 twistMeasureAxis;
Quaternion.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis);
//By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value.
float y, x;
Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y);
Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x);
var angle = (float)Math.Atan2(y, x);
//Compute goal velocity.
error = GetDistanceFromGoal(angle);
float absErrorOverDt = Math.Abs(error / dt);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out usedSoftness);
biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction;
biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity);
}
else
{
biasVelocity = settings.velocityMotor.goalVelocity;
usedSoftness = settings.velocityMotor.softness / dt;
error = 0;
}
//The nice thing about this approach is that the jacobian entry doesn't flip.
//Instead, the error can be negative due to the use of Atan2.
//This is important for limits which have a unique high and low value.
//Compute the jacobian.
Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB);
if (jacobianB.LengthSquared() < Toolbox.Epsilon)
{
//A nasty singularity can show up if the axes are aligned perfectly.
//In a 'real' situation, this is impossible, so just ignore it.
isActiveInSolver = false;
return;
}
jacobianB.Normalize();
jacobianA.X = -jacobianB.X;
jacobianA.Y = -jacobianB.Y;
jacobianA.Z = -jacobianB.Z;
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
}
else
{
entryA = 0;
}
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
{
entryB = 0;
}
//Compute the inverse mass matrix
velocityToImpulse = 1 / (usedSoftness + entryA + entryB);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray()
{
Position = startingTransform.Position, Direction = Toolbox.UpVector
};
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit()
{
Location = startingTransform.Position, Normal = new Vector3(), T = 0
};
return(true);
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {
Translation = worldTransform.Position
};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
///<summary>
/// Constructs a new morphable entity.
///</summary>
///<param name="shape">Shape to use with the entity.</param>
///<param name="mass">Mass of the entity.</param>
/// <param name="inertiaTensor">Inertia tensor of the entity.</param>
public MorphableEntity(EntityShape shape, float mass, Matrix3x3 inertiaTensor)
: base(shape, mass, inertiaTensor)
{
}
public abstract void SetUniform(string name, Matrix3x3 value);
public static void Validate(this Matrix3x3 m)
{
m.Right.Validate();
m.Up.Validate();
m.Backward.Validate();
}
protected internal void Initialize(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor)
{
CollisionInformation = collisionInformation;
if (mass > 0)
{
BecomeDynamic(mass, inertiaTensor);
}
else
{
BecomeKinematic();
}
collisionInformation.Entity = this;
}
