| 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; | 
| 133 | 
  | 
 | 
| 134 | 
  | 
    A = rot * A; //? A = A* rot.transpose(); | 
| 135 | 
  | 
   | 
| 136 | 
< | 
} | 
| 136 | 
> | 
  } | 
| 137 | 
  | 
 | 
| 138 | 
  | 
 | 
| 139 | 
  | 
} |