static void _calculateTransformMatrix(Matrix4 transform, MyVector3 pos, MyQuaternion ori)
{
transform.data[0] = 1 - 2 * ori.j * ori.j -
2 * ori.k * ori.k;
transform.data[1] = 2 * ori.i * ori.j -
2 * ori.r * ori.k;
transform.data[2] = 2 * ori.i * ori.k +
2 * ori.r * ori.j;
transform.data[3] = pos.x;
transform.data[4] = 2 * ori.i * ori.j +
2 * ori.r * ori.k;
transform.data[5] = 1 - 2 * ori.i * ori.i -
2 * ori.k * ori.k;
transform.data[6] = 2 * ori.j * ori.k -
2 * ori.r * ori.i;
transform.data[7] = pos.y;
transform.data[8] = 2 * ori.i * ori.k -
2 * ori.r * ori.j;
transform.data[9] = 2 * ori.j * ori.k +
2 * ori.r * ori.i;
transform.data[10] = 1 - 2 * ori.i * ori.i -
2 * ori.j * ori.j;
transform.data[11] = pos.z;
}
public void rotateByVector(MyVector3 vector)
{
MyQuaternion q = new MyQuaternion(0, vector.x, vector.y, vector.z);
MyQuaternion thisq = new MyQuaternion(this.r, this.i, this.j, this.k);
thisq *= q;
this.r = thisq.r;
this.i = thisq.i;
this.j = thisq.j;
this.k = thisq.k;
}
public void addScaledVector(MyVector3 vector, real scale)
{
MyQuaternion q = new MyQuaternion(0,
vector.x * scale,
vector.y * scale,
vector.z * scale);
q *= this;
r += q.r * ((real)0.5);
i += q.i * ((real)0.5);
j += q.j * ((real)0.5);
k += q.k * ((real)0.5);
}
public RigidBody()
{
this.inverseMass = new real();
this.inverseInertiaTensor = new Matrix3();
this.linearDamping = new real();
this.angularDamping = new real();
this.position = new MyVector3();
this.orientation = new MyQuaternion();
this.velocity = new MyVector3();
this.rotation = new MyVector3();
this.inverseInertiaTensorWorld = new Matrix3();
this.motion = new real();
this.isAwake = new bool();
this.canSleep = new bool();
this.transformMatrix = new Matrix4();
this.forceAccum = new MyVector3();
this.torqueAccum = new MyVector3();
this.acceleration = new MyVector3();
this.lastFrameAcceleration = new MyVector3();
}
public void getOrientation(MyQuaternion orientation)
{
orientation = this.orientation;
}
public void setOrientation(MyQuaternion orientation)
{
this.orientation = orientation;
this.orientation.normalise();
}
public MyQuaternion(MyQuaternion orientation)
{
this.orientation = orientation;
}