| 1 |
< |
/* |
| 1 |
> |
/* |
| 2 |
|
* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved. |
| 3 |
|
* |
| 4 |
|
* The University of Notre Dame grants you ("Licensee") a |
| 43 |
|
|
| 44 |
|
namespace oopse { |
| 45 |
|
|
| 46 |
< |
void DLM::doRotate(StuntDouble* sd, Vector3d& ji, double dt) { |
| 47 |
< |
double dt2 = 0.5 * dt; |
| 48 |
< |
double angle; |
| 46 |
> |
void DLM::doRotate(StuntDouble* sd, Vector3d& ji, RealType dt) { |
| 47 |
> |
RealType dt2 = 0.5 * dt; |
| 48 |
> |
RealType angle; |
| 49 |
|
|
| 50 |
|
RotMat3x3d A = sd->getA(); |
| 51 |
|
Mat3x3d I = sd->getI(); |
| 53 |
|
// use the angular velocities to propagate the rotation matrix a full time step |
| 54 |
|
if (sd->isLinear()) { |
| 55 |
|
|
| 56 |
< |
int i = sd->linearAxis(); |
| 57 |
< |
int j = (i+1)%3; |
| 58 |
< |
int k = (i+2)%3; |
| 56 |
> |
int i = sd->linearAxis(); |
| 57 |
> |
int j = (i+1)%3; |
| 58 |
> |
int k = (i+2)%3; |
| 59 |
|
|
| 60 |
< |
angle = dt2 * ji[j] / I(j, j); |
| 61 |
< |
rotateStep( k, i, angle, ji, A ); |
| 60 |
> |
angle = dt2 * ji[j] / I(j, j); |
| 61 |
> |
rotateStep( k, i, angle, ji, A ); |
| 62 |
|
|
| 63 |
< |
angle = dt * ji[k] / I(k, k); |
| 64 |
< |
rotateStep( i, j, angle, ji, A); |
| 63 |
> |
angle = dt * ji[k] / I(k, k); |
| 64 |
> |
rotateStep( i, j, angle, ji, A); |
| 65 |
|
|
| 66 |
< |
angle = dt2 * ji[j] / I(j, j); |
| 67 |
< |
rotateStep( k, i, angle, ji, A ); |
| 66 |
> |
angle = dt2 * ji[j] / I(j, j); |
| 67 |
> |
rotateStep( k, i, angle, ji, A ); |
| 68 |
|
|
| 69 |
|
} else { |
| 70 |
< |
// rotate about the x-axis |
| 71 |
< |
angle = dt2 * ji[0] / I(0, 0); |
| 72 |
< |
rotateStep( 1, 2, angle, ji, A ); |
| 70 |
> |
// rotate about the x-axis |
| 71 |
> |
angle = dt2 * ji[0] / I(0, 0); |
| 72 |
> |
rotateStep( 1, 2, angle, ji, A ); |
| 73 |
|
|
| 74 |
< |
// rotate about the y-axis |
| 75 |
< |
angle = dt2 * ji[1] / I(1, 1); |
| 76 |
< |
rotateStep( 2, 0, angle, ji, A ); |
| 74 |
> |
// rotate about the y-axis |
| 75 |
> |
angle = dt2 * ji[1] / I(1, 1); |
| 76 |
> |
rotateStep( 2, 0, angle, ji, A ); |
| 77 |
|
|
| 78 |
< |
// rotate about the z-axis |
| 79 |
< |
angle = dt * ji[2] / I(2, 2); |
| 80 |
< |
sd->addZangle(angle); |
| 81 |
< |
rotateStep( 0, 1, angle, ji, A); |
| 78 |
> |
// rotate about the z-axis |
| 79 |
> |
angle = dt * ji[2] / I(2, 2); |
| 80 |
> |
sd->addZangle(angle); |
| 81 |
> |
rotateStep( 0, 1, angle, ji, A); |
| 82 |
|
|
| 83 |
< |
// rotate about the y-axis |
| 84 |
< |
angle = dt2 * ji[1] / I(1, 1); |
| 85 |
< |
rotateStep( 2, 0, angle, ji, A ); |
| 83 |
> |
// rotate about the y-axis |
| 84 |
> |
angle = dt2 * ji[1] / I(1, 1); |
| 85 |
> |
rotateStep( 2, 0, angle, ji, A ); |
| 86 |
|
|
| 87 |
< |
// rotate about the x-axis |
| 88 |
< |
angle = dt2 * ji[0] / I(0, 0); |
| 89 |
< |
rotateStep( 1, 2, angle, ji, A ); |
| 87 |
> |
// rotate about the x-axis |
| 88 |
> |
angle = dt2 * ji[0] / I(0, 0); |
| 89 |
> |
rotateStep( 1, 2, angle, ji, A ); |
| 90 |
|
|
| 91 |
|
} |
| 92 |
|
|
| 93 |
|
sd->setA( A ); |
| 94 |
< |
} |
| 94 |
> |
} |
| 95 |
|
|
| 96 |
|
|
| 97 |
< |
void DLM::rotateStep(int axes1, int axes2, double angle, Vector3d& ji, RotMat3x3d& A) { |
| 97 |
> |
void DLM::rotateStep(int axes1, int axes2, RealType angle, Vector3d& ji, RotMat3x3d& A) { |
| 98 |
|
|
| 99 |
< |
double sinAngle; |
| 100 |
< |
double cosAngle; |
| 101 |
< |
double angleSqr; |
| 102 |
< |
double angleSqrOver4; |
| 103 |
< |
double top, bottom; |
| 99 |
> |
RealType sinAngle; |
| 100 |
> |
RealType cosAngle; |
| 101 |
> |
RealType angleSqr; |
| 102 |
> |
RealType angleSqrOver4; |
| 103 |
> |
RealType top, bottom; |
| 104 |
|
|
| 105 |
|
RotMat3x3d tempA(A); // initialize the tempA |
| 106 |
|
Vector3d tempJ(0.0); |
| 133 |
|
|
| 134 |
|
A = rot * A; //? A = A* rot.transpose(); |
| 135 |
|
|
| 136 |
< |
} |
| 136 |
> |
} |
| 137 |
|
|
| 138 |
|
|
| 139 |
|
} |
