| 118 | } |
| 119 | |
| 120 | void Motor3D::GetTransformMatrices(Transform3D *M, Transform3D *Minv) const |
| 121 | { |
| 122 | float vx2 = v.x * v.x; |
| 123 | float vy2 = v.y * v.y; |
| 124 | float vz2 = v.z * v.z; |
| 125 | |
| 126 | float A00 = 1.0F - (vy2 + vz2) * 2.0F; |
| 127 | float A11 = 1.0F - (vz2 + vx2) * 2.0F; |
| 128 | float A22 = 1.0F - (vx2 + vy2) * 2.0F; |
| 129 | float A01 = v.x * v.y; |
| 130 | float A02 = v.z * v.x; |
| 131 | float A12 = v.y * v.z; |
| 132 | float A03 = v.y * m.z - v.z * m.y; |
| 133 | float A13 = v.z * m.x - v.x * m.z; |
| 134 | float A23 = v.x * m.y - v.y * m.x; |
| 135 | |
| 136 | float B01 = v.z * v.w; |
| 137 | float B20 = v.y * v.w; |
| 138 | float B12 = v.x * v.w; |
| 139 | float B03 = m.x * v.w - v.x * m.w; |
| 140 | float B13 = m.y * v.w - v.y * m.w; |
| 141 | float B23 = m.z * v.w - v.z * m.w; |
| 142 | |
| 143 | M->Set( A00, (A01 - B01) * 2.0F, (A02 + B20) * 2.0F, (A03 + B03) * 2.0F, |
| 144 | (A01 + B01) * 2.0F, A11, (A12 - B12) * 2.0F, (A13 + B13) * 2.0F, |
| 145 | (A02 - B20) * 2.0F, (A12 + B12) * 2.0F, A22, (A23 + B23) * 2.0F); |
| 146 | |
| 147 | Minv->Set( A00, (A01 + B01) * 2.0F, (A02 - B20) * 2.0F, (A03 - B03) * 2.0F, |
| 148 | (A01 - B01) * 2.0F, A11, (A12 + B12) * 2.0F, (A13 - B13) * 2.0F, |
| 149 | (A02 + B20) * 2.0F, (A12 - B12) * 2.0F, A22, (A23 - B23) * 2.0F); |
| 150 | } |
| 151 | |
| 152 | Motor3D& Motor3D::SetTransformMatrix(const Transform3D& M) |
| 153 | { |