| 120 |
|
|
| 121 |
|
pos = datom->getPos(); |
| 122 |
|
vel = datom->getVel(); |
| 123 |
< |
frc = datom->getFrc(); |
| 124 |
< |
trq = datom->getTrq(); |
| 123 |
> |
|
| 124 |
|
j = datom->getJ(); |
| 125 |
|
I = datom->getI(); |
| 126 |
|
A = datom->getA(); |
| 127 |
+ |
|
| 128 |
|
skewMat(0, 0) = 0; |
| 129 |
|
skewMat(0, 1) = j[2] / I(2, 2); |
| 130 |
|
skewMat(0, 2) = -j[1] / I(1, 1); |
| 134 |
|
skewMat(2, 0) = j[1] / I(1, 1); |
| 135 |
|
skewMat(2, 1) = -j[0] / I(0, 0); |
| 136 |
|
skewMat(2, 2) = 0; |
| 137 |
– |
|
| 137 |
|
Mat3x3d mat = (A * skewMat).transpose(); |
| 138 |
|
|
| 139 |
|
// We need A^T to convert from body-fixed to space-fixed: |