| 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) { | 
| 46 | > | void DLM::doRotate(StuntDouble* sd, Vector3d& ji, double dt) { | 
| 47 |  | double dt2 = 0.5 * dt; | 
| 48 |  | double angle; | 
| 49 |  |  | 
| 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, double angle, Vector3d& ji, RotMat3x3d& A) { | 
| 98 |  |  | 
| 99 |  | double sinAngle; | 
| 100 |  | double cosAngle; | 
| 109 |  |  | 
| 110 |  | // use a small angle aproximation for sin and cosine | 
| 111 |  |  | 
| 112 | < | angleSqr = angle * angle; | 
| 113 | < | angleSqrOver4 = angleSqr / 4.0; | 
| 114 | < | top = 1.0 - angleSqrOver4; | 
| 115 | < | bottom = 1.0 + angleSqrOver4; | 
| 112 | > | //angleSqr = angle * angle; | 
| 113 | > | //angleSqrOver4 = angleSqr / 4.0; | 
| 114 | > | //top = 1.0 - angleSqrOver4; | 
| 115 | > | //bottom = 1.0 + angleSqrOver4; | 
| 116 |  |  | 
| 117 | < | cosAngle = top / bottom; | 
| 118 | < | sinAngle = angle / bottom; | 
| 119 | < |  | 
| 117 | > | //cosAngle = top / bottom; | 
| 118 | > | //sinAngle = angle / bottom; | 
| 119 | > | cosAngle = cos(angle); | 
| 120 | > | sinAngle = sin(angle); | 
| 121 |  | rot(axes1, axes1) = cosAngle; | 
| 122 |  | rot(axes2, axes2) = cosAngle; | 
| 123 |  |  | 
| 133 |  |  | 
| 134 |  | A = rot * A; //? A = A* rot.transpose(); | 
| 135 |  |  | 
| 136 | < | } | 
| 136 | > | } | 
| 137 |  |  | 
| 138 |  |  | 
| 139 |  | } |