<< 
      Calculus`VectorAnalysis`

axisAngleRotate[A_, theta_, 
        P_]  :=  (
ax = A[[1]]; ay = A[[2]]; 
        az = A[[3]]; 
AAT  =  {
ax {ax, ay, az}, 
ay {ax, ay, az}, 
az {ax, 
                ay, az}
}; 
Astar = {{0, -az, ay}, {az, 0, -ax}, {-ay, ax, 0}}; 
M = 
          AAT  +  
            Cos[theta] (IdentityMatrix[3] - AAT) + 
Sin[
                theta] Astar; 
M . 
          P
)

 (* 
      object data structure *) 
 (* 
      constants :  mass,  inertia tensor,  
      inverse of inertia tensor;  state variables :  position,  
      orientation,  momentum,  angular momentum;  
      auxiliary variables :  velocity,  
      angular velocity,  
velocity,  rotation - dot,  
      force,  torque *) 
object  = {
 (* 
            geometry definition *) 
{{-4, 
              4}, {-2, 2}, {-1, 1}},               (* 
            min/max in x, y, z*) 
 (* 
            constants *) 
20,                                                                          (* 
            mass *) 
{{40, 0, 0}, {0, 20, 0}, {0, 0, 
              10}},   (* 
            I *) 
{{1, 0, 0}, {0, 1, 0}, {0, 0, 
              1}},   (* I inverse *) 
 (* 
            state variables *) 
{0, 0, 
            0},                                                         (* 
            position *) 
{{1, 0, 0}, {0, 1, 0}, {0, 0, 
              1}},  (* orientation *) 
{0, 0, 
            0},                                                          (* 
            momentum *) 
{0, 0, 
            0},                                                         (* 
            angular momentum *) 
 (* 
            auxiliary variables *) 
{0, 0, 
            0},                                                          (* 
            velocity *) 
{{1, 0, 0}, {0, 1, 0}, {0, 0, 
              1}},   (* I inverse (time) *) 
{0, 
            0, 0},                                                          (* 
            angular velocity *) 
 (* 
            derivative variables *) 
{0, 0, 
            0},                                                          (* 
            velocity *) 
{{1, 0, 0}, {0, 1, 0}, {0, 0, 
              1}},   (* orientation dot *) 
{0, 0, 
            0},                                                          (* 
            force *) 
{0, 0, 
            0}                                                         (* 
            torque *) 
};

object  = 
        ReplacePart[object, Inverse[object[[3]]], 4];

mass[obj_]  :=  
      obj[[2]]
inertia[obj_] := obj[[3]]
inertiaInv[obj_] := obj[[4]]
position[obj_]  :=  obj[[5]]
orient[obj_]  :=  obj[[6]]
momentum[obj_]  :=  obj[[7]]
angularMomentum[obj_]  :=  obj[[8]]
velocityAux[obj_]  :=  obj[[9]]
Iinv[obj_]  :=  obj[[10]]
omega[obj_] := obj[[11]]
velocity[obj_]  :=  obj[[12]]
orientDot[obj_] := obj[[13]]
force[obj_] := obj[[14]]
torque[obj_] := obj[[15]]
setPosition[obj_, value_] := 
      ReplacePart[obj, value, 5]
setOrient[obj_, value_]  :=  
      ReplacePart[obj, value, 6]
setMomentum[obj_, value_]  :=  
      ReplacePart[obj, value, 7]
setAngularMomentum[obj_, value_] := 
      ReplacePart[obj, value, 8]
setVelocityAux[obj_, value_] := 
      ReplacePart[obj, value, 9]
setIinverse[obj_, value_]  := 
      ReplacePart[obj, value, 10]
setOmega[obj_, value_]  := 
      ReplacePart[obj, value, 11]
setVelocity[obj_, value_] := 
      ReplacePart[obj, value, 12]
setOrientDot[obj_, value_] := 
      ReplacePart[obj, value, 13]
setForce[obj_, value_]  :=  
        ReplacePart[obj, value, 14];
setTorque[obj_, value_]  := 
      ReplacePart[obj, value, 15]
addForce[obj_, value_] :=  
      setForce[obj, force[obj] + value]
addTorque[obj_, value_] := 
      setTorque[obj, torque[obj] + value]

computeAux[
        obj_]  :=  (
a1 = 
          setVelocityAux[obj, 
            momentum[obj]/mass[obj]]; 
a2 = 
          setIinverse[a1, 
            orient[a1] . inertiaInv[a1] . 
              Inverse[orient[a1]]]; 
setOmega[a2, 
          inertiaInv[a2] . 
            angularMomentum[a2]]
)

(* 
      compute additional derivative information *) 
computeDerivative[
        obj_]:= (
d1 = 
          setVelocity[obj, velocityAux[obj]]; 
dw = 
          omega[d1]; 
d2 = 
          Transpose[orient[d1]]; 
setOrientDot[d1, 
          Transpose[{CrossProduct[dw, d2[[1]]], 
              CrossProduct[dw, d2[[2]]], 
              CrossProduct[dw, 
                d2[[3]]]}]]
)

(* 
      rotate orientation matrix with axis - 
        angle rotation *) 
updateObject[obj_, 
        dt_] := (
u1 =  
          setPosition[obj, 
            position[obj] + velocity[obj]*dt]; 
dw = 
          omega[u1]; 
ds  =  
          DotProduct[dw, dw]; 
drot = 
          If[ds ≠ 0, 
dw = 
              dw/Sqrt[ds]; 
drot = 
              orient[u1]; 
{
axisAngleRotate[dw, ds*dt, 
                drot[[1]]], 
axisAngleRotate[dw, ds*dt, 
                drot[[2]]], 
axisAngleRotate[dw, ds*dt, 
                drot[[3]]]
}, 
orient[
              u1]
]; 
u2 = 
          setOrient[u1, drot]; 
u3 = 
          setMomentum[u2, 
            momentum[u2] + force[u2]*dt]; 
u4  =  
          setAngularMomentum[u3, 
            angularMomentum[u3] + torque[u3]*dt]; 
u5  =  
          setForce[u4, {0, 0, 0}]; 
u6  =  
          setTorque[u5, {0, 0, 0}]; 
 (* 
          normalize orient matrix *) 
u7 = 
          orient[u6]; 
u8 = 
          u7[[1]]; 
len = 
          Sqrt[u8[[1]]*u8[[1]] + u8[[2]]*u8[[2]] + 
              u8[[3]]*u8[[3]]]; 
u91 = 
          u8/len; 
u8 = 
          u7[[2]]; 
len = 
          Sqrt[u8[[1]]*u8[[1]] + u8[[2]]*u8[[2]] + 
              u8[[3]]*u8[[3]]]; 
u92 = 
          u8/len; 
u8 = 
          u7[[3]]; 
len = 
          Sqrt[u8[[1]]*u8[[1]] + u8[[2]]*u8[[2]] + 
              u8[[3]]*u8[[3]]]; 
u93 = 
          u8/len; 
u10 = 
          setOrient[u6, {u91, u92, u93}]
)

display[
        obj_] :=  (
x0 = obj[[1]][[1]][[1]]; 
x1  = obj[[1]][[1]][[2]]; 
y0  = obj[[1]][[2]][[1]]; 
y1  = obj[[1]][[2]][[2]]; 
z0 = obj[[1]][[3]][[1]]; 
z1  = obj[[1]][[3]][[2]]; 
m = 
          orient[object]; 
pos = 
          position[object]; 

p000  = 
          m . {x0, y0, z0} + pos; 
p001 = 
          m . {x0, y0, z1} + pos; 
p010 = 
          m . {x0, y1, z0} + pos; 
p011 = 
          m . {x0, y1, z1} + pos; 
p100 = 
          m . {x1, y0, z0} + pos; 
p101 = 
          m . {x1, y0, z1} + pos; 
p110 = 
          m . {x1, y1, z0} + pos; 
p111 = 
          m . {x1, y1, z1} + 
            pos; 

polys = {
Polygon[{p000, p100, p110, p010}], 
            Polygon[{p001, p101, p111, p011}], 
            Polygon[{p000, p001, p101, p100}], 
            Polygon[{p010, p011, p111, p110}], 
            Polygon[{p000, p010, p011, p001}], 
            Polygon[{p100, p110, p111, p101}]}; 
Show[
          Graphics3D[polys], 
          PlotRange -> {{-10, 10}, {-10, 10}, {-10, 
                10}}]
)

display[object]

applyForce[obj_, p_, 
        f_] := (
af1 = 
          addForce[obj, f]; 
addTorque[af1, 
          CrossProduct[p, f]]
)