Code: Rigid Body using Quaternions

State of rigid body = Y = x,q,P,L and Y' = v, 1/2*omega*q, F, torque
More detailed documentation later.

Mat3d R = vl_1; Vec3d P = vl_0; Vec3d L = vl_0; Vec3d p = Vec3d(-1,0,0); Vec3d F = Vec3d(.05,10.0,0); Mat3d Ibody = Mat3d(2.0/5.0,0,0,0,2.0/5.0,0,0,0,2.0/5.0); Mat3d osr; Vec3d torque; Vec4d q = vl_0;

if (mytime > 0.0) F = Vec3d(0,0,0);
//Update x using v
newpartvel[i] = P;
newpartpos[i] = newpartpos[i] + howFast*newpartvel[i];
q = mat2quat(R);
//update R using omegastar*R
Mat3d Rt(R[0][0],R[1][0],R[2][0],R[0][1],R[1][1],R[2][1],R[0][2],R[1][2],R[2][2]);
Mat3d It = R * Ibody * Rt;
Mat3d Iinv = inv(It);
Vec3d omega = Iinv*L;
Vec4d omega4 = Vec4d(0,omega[0],omega[1],omega[2]);
q = q + howFast*(quatmult(((.5)*omega4),q));
R = quat2mat(q/quatmag(q));
//update P using F
P = P + howFast*(F-(Vec3d(0,gravity,0)));
//update L using torque
Vec3d pd = p - newpartpos[i];
torque = cross(pd,F);
//torque = Vec3d(pd[1]*F[2] - pd[2]*F[1], pd[2]*F[0] - pd[0]*F[2], pd[0]*F[1] - pd[1]*F[0]);
L = L + howFast*(torque);
newpartpos[1] = R*Vec3d(-1,0,0) + newpartpos[i];;
newpartpos[2] = R*Vec3d(1,0,0) + newpartpos[i];
newpartpos[3] = R*Vec3d(0,1,0) + newpartpos[i];
newpartpos[4] = R*Vec3d(0,-1,0) + newpartpos[i];
newpartpos[5] = R*Vec3d(0,0,1) + newpartpos[i];
newpartpos[6] = R*Vec3d(0,0,-1) + newpartpos[i];


}


mytime=mytime+howFast;