// Start is called before the first frame update
void Start()
{
worldTransformMatrix = new MadeMatrix4x4(
1.0f, 0.0f, 0.0f, transform.position.x,
0.0f, 1.0f, 0.0f, transform.position.y,
0.0f, 0.0f, 1.0f, transform.position.z,
0.0f, 0.0f, 0.0f, 1.0f
);
worldTransformMatrix.calculateInv();
worldTranformInverseMatrix = new MadeMatrix4x4();
worldTranformInverseMatrix.matrix = worldTransformMatrix.invMatrix;
torqueContainer.worldCenterOfMass = transform.position;
torqueContainer.localCenterOfMass = Vector3.zero;
SetMass(forces.startingMass);
particle3DTransform.position = transform.position;
Quaternion rot = gameObject.transform.rotation;
Vector4 quatVals = new Vector4(rot.x, rot.y, rot.z, 1);
particle3DTransform.rotation = new MadeQuaternion(quatVals.x, quatVals.y, quatVals.z);
UpdateInertia();
colHull = this.gameObject.GetComponent <CollisionHull2D>();
}
public MadeMatrix4x4 zero()
{
MadeMatrix4x4 zero = new MadeMatrix4x4(
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0
);
return(zero);
}
// cube: 𝐼 = 1/6 * mass * size^2
MadeMatrix4x4 HollowSphereTensor(float radius, float mass)
{
float inputVal = 0.66f * mass * radius * radius;
MadeMatrix4x4 newMat = new MadeMatrix4x4(
inputVal, 0, 0, 0,
0, inputVal, 0, 0,
0, 0, inputVal, 0,
0, 0, 0, 1
);
return(newMat);
}
MadeMatrix4x4 SolidBoxTensor(float height, float width, float length, float mass)
{
float col1Input = 0.083f * mass * (height * height + length * length);
float col2Input = 0.083f * mass * (length * length + width * width);
float col3Input = 0.083f * mass * (width * width + height * height);
MadeMatrix4x4 newMat = new MadeMatrix4x4(
col1Input, 0, 0, 0,
0, col2Input, 0, 0,
0, 0, col3Input, 0,
0, 0, 0, 1
);
return(newMat);
}
// axis parallel to third lacal basis ***FOUND IN THE SLIDES #18***
MadeMatrix4x4 SolidConeTensor(float radius, float height, float mass)
{
float col1Input = 0.6f * mass * height * height + 0.15f * mass * radius * radius;
float col2Input = col1Input;
float col3Input = 0.3f * mass * radius * radius;
MadeMatrix4x4 newMat = new MadeMatrix4x4(
col1Input, 0, 0, 0,
0, col2Input, 0, 0,
0, 0, col3Input, 0,
0, 0, 0, 1
);
return(newMat);
}
MadeMatrix4x4 SolidCylinderTensor(float radius, float height, float mass)
{
float col1Input = 0.083f * mass * (3 * radius * radius + height * height);
float col2Input = col1Input;
float col3Input = 0.5f * mass * radius * radius;
MadeMatrix4x4 newMat = new MadeMatrix4x4(
col1Input, 0, 0, 0,
0, col2Input, 0, 0,
0, 0, col3Input, 0,
0, 0, 0, 1
);
return(newMat);
}
void UpdateWorldMatrix()
{
float cosX = Mathf.Cos(particle3DTransform.eulerAngle.x * Mathf.Deg2Rad);
float sinX = Mathf.Sin(particle3DTransform.eulerAngle.x * Mathf.Deg2Rad);
float cosY = Mathf.Cos(particle3DTransform.eulerAngle.y * Mathf.Deg2Rad);
float sinY = Mathf.Sin(particle3DTransform.eulerAngle.y * Mathf.Deg2Rad);
float cosZ = Mathf.Cos(particle3DTransform.eulerAngle.z * Mathf.Deg2Rad);
float sinZ = Mathf.Sin(particle3DTransform.eulerAngle.z * Mathf.Deg2Rad);
worldTransformMatrix = new MadeMatrix4x4(
cosY * cosZ, -cosY * sinZ, sinY, transform.position.x,
sinX * sinY * cosZ + cosX * sinZ, -sinX * sinY * sinZ + cosX * cosZ, -sinX * cosY, transform.position.y,
-cosX * sinY * cosZ + sinX * sinZ, cosX * sinY * sinZ + sinX * cosZ, cosX * cosY, transform.position.z,
0.0f, 0.0f, 0.0f, 1.0f
);
worldTransformMatrix.calculateInv();
worldTranformInverseMatrix.matrix = worldTransformMatrix.invMatrix;
}
public static MadeMatrix4x4 operator *(MadeMatrix4x4 myQuad, MadeMatrix4x4 otherQuad)
{
MadeMatrix4x4 newMat = new MadeMatrix4x4();
/*
* 0, 1, 2, 3,
* 4, 5, 6, 7,
* 8, 9,10,11,
* 12,13,14,15
*/
/*
* 00,01,02,03
* 10,11,12,13
* 20,21,22,23
* 30,31,32,33
*/
#region left quad vals
float leftQuad00 = myQuad.matrix[0];
float leftQuad01 = myQuad.matrix[1];
float leftQuad02 = myQuad.matrix[2];
float leftQuad03 = myQuad.matrix[3];
float leftQuad10 = myQuad.matrix[4];
float leftQuad11 = myQuad.matrix[5];
float leftQuad12 = myQuad.matrix[6];
float leftQuad13 = myQuad.matrix[7];
float leftQuad20 = myQuad.matrix[8];
float leftQuad21 = myQuad.matrix[9];
float leftQuad22 = myQuad.matrix[10];
float leftQuad23 = myQuad.matrix[11];
float leftQuad30 = myQuad.matrix[12];
float leftQuad31 = myQuad.matrix[13];
float leftQuad32 = myQuad.matrix[14];
float leftQuad33 = myQuad.matrix[15];
#endregion
#region right quad vals
float rightQuad00 = otherQuad.matrix[0];
float rightQuad01 = otherQuad.matrix[1];
float rightQuad02 = otherQuad.matrix[2];
float rightQuad03 = otherQuad.matrix[3];
float rightQuad10 = otherQuad.matrix[4];
float rightQuad11 = otherQuad.matrix[5];
float rightQuad12 = otherQuad.matrix[6];
float rightQuad13 = otherQuad.matrix[7];
float rightQuad20 = otherQuad.matrix[8];
float rightQuad21 = otherQuad.matrix[9];
float rightQuad22 = otherQuad.matrix[10];
float rightQuad23 = otherQuad.matrix[11];
float rightQuad30 = otherQuad.matrix[12];
float rightQuad31 = otherQuad.matrix[13];
float rightQuad32 = otherQuad.matrix[14];
float rightQuad33 = otherQuad.matrix[15];
#endregion
newMat.matrix[0] = leftQuad00 * rightQuad00 + leftQuad01 * rightQuad10 + leftQuad02 * rightQuad20 + leftQuad03 * rightQuad30;
newMat.matrix[1] = leftQuad00 * rightQuad01 + leftQuad01 * rightQuad11 + leftQuad02 * rightQuad21 + leftQuad03 * rightQuad31;
newMat.matrix[2] = leftQuad00 * rightQuad02 + leftQuad01 * rightQuad12 + leftQuad02 * rightQuad22 + leftQuad03 * rightQuad32;
newMat.matrix[3] = leftQuad00 * rightQuad03 + leftQuad01 * rightQuad13 + leftQuad02 * rightQuad23 + leftQuad03 * rightQuad33;
newMat.matrix[4] = leftQuad10 * rightQuad00 + leftQuad11 * rightQuad10 + leftQuad12 * rightQuad20 + leftQuad13 * rightQuad30;
newMat.matrix[5] = leftQuad10 * rightQuad01 + leftQuad11 * rightQuad11 + leftQuad12 * rightQuad21 + leftQuad13 * rightQuad31;
newMat.matrix[6] = leftQuad10 * rightQuad02 + leftQuad11 * rightQuad12 + leftQuad12 * rightQuad22 + leftQuad13 * rightQuad32;
newMat.matrix[7] = leftQuad10 * rightQuad03 + leftQuad11 * rightQuad13 + leftQuad12 * rightQuad23 + leftQuad13 * rightQuad33;
newMat.matrix[8] = leftQuad20 * rightQuad00 + leftQuad21 * rightQuad10 + leftQuad22 * rightQuad20 + leftQuad23 * rightQuad30;
newMat.matrix[9] = leftQuad20 * rightQuad01 + leftQuad21 * rightQuad11 + leftQuad22 * rightQuad21 + leftQuad23 * rightQuad31;
newMat.matrix[10] = leftQuad20 * rightQuad02 + leftQuad21 * rightQuad12 + leftQuad22 * rightQuad22 + leftQuad23 * rightQuad32;
newMat.matrix[11] = leftQuad20 * rightQuad03 + leftQuad21 * rightQuad13 + leftQuad22 * rightQuad23 + leftQuad23 * rightQuad33;
newMat.matrix[12] = leftQuad30 * rightQuad00 + leftQuad31 * rightQuad10 + leftQuad32 * rightQuad20 + leftQuad33 * rightQuad30;
newMat.matrix[13] = leftQuad30 * rightQuad01 + leftQuad31 * rightQuad11 + leftQuad32 * rightQuad21 + leftQuad33 * rightQuad31;
newMat.matrix[14] = leftQuad30 * rightQuad02 + leftQuad31 * rightQuad12 + leftQuad32 * rightQuad22 + leftQuad33 * rightQuad32;
newMat.matrix[15] = leftQuad30 * rightQuad03 + leftQuad31 * rightQuad13 + leftQuad32 * rightQuad23 + leftQuad33 * rightQuad33;
return(newMat);
}
