| [aa639ea] | 1 | #include "transformation.h" |
|---|
| 2 | #include <math.h> |
|---|
| 3 | #include <stdexcept> |
|---|
| 4 | |
|---|
| 5 | void RotateX(const double ang_x,Point3D &a_point) |
|---|
| 6 | { |
|---|
| 7 | double sinA_ = sin(ang_x); |
|---|
| 8 | double cosA_ = cos(ang_x); |
|---|
| 9 | double pointX_ = a_point.getX(); |
|---|
| 10 | double pointY_ = a_point.getY(); |
|---|
| 11 | double pointZ_ = a_point.getZ(); |
|---|
| 12 | |
|---|
| 13 | double x_new_ = pointX_; |
|---|
| 14 | double y_new_ = pointY_*cosA_ - pointZ_*sinA_; |
|---|
| 15 | double z_new_ = pointY_*sinA_ - pointZ_*cosA_; |
|---|
| 16 | |
|---|
| 17 | a_point.set(x_new_, y_new_, z_new_); |
|---|
| 18 | } |
|---|
| 19 | |
|---|
| 20 | void RotateY(const double ang_y,Point3D &a_point) |
|---|
| 21 | { |
|---|
| 22 | double sinA_ = sin(ang_y); |
|---|
| 23 | double cosA_ = cos(ang_y); |
|---|
| 24 | double pointX_ = a_point.getX(); |
|---|
| 25 | double pointY_ = a_point.getY(); |
|---|
| 26 | double pointZ_ = a_point.getZ(); |
|---|
| 27 | |
|---|
| 28 | double x_new_ = pointX_*cosA_ + pointZ_*sinA_; |
|---|
| 29 | double y_new_ = pointY_; |
|---|
| 30 | double z_new_ = -pointX_*sinA_ + pointZ_*cosA_; |
|---|
| 31 | |
|---|
| 32 | a_point.set(x_new_, y_new_, z_new_); |
|---|
| 33 | } |
|---|
| 34 | |
|---|
| 35 | void RotateZ(const double ang_z,Point3D &a_point) |
|---|
| 36 | { |
|---|
| 37 | double sinA_= sin(ang_z); |
|---|
| 38 | double cosA_ = cos(ang_z); |
|---|
| 39 | double pointX_ = a_point.getX(); |
|---|
| 40 | double pointY_ = a_point.getY(); |
|---|
| 41 | double pointZ_ = a_point.getZ(); |
|---|
| 42 | |
|---|
| 43 | double x_new_ = pointX_*cosA_ - pointY_*sinA_; |
|---|
| 44 | double y_new_ = pointX_*sinA_+ pointY_*cosA_; |
|---|
| 45 | double z_new_ = pointZ_; |
|---|
| 46 | |
|---|
| 47 | a_point.set(x_new_, y_new_, z_new_); |
|---|
| 48 | } |
|---|
| 49 | |
|---|
| 50 | void Translate(const double trans_x, const double trans_y, const double trans_z, Point3D &a_point) |
|---|
| 51 | { |
|---|
| 52 | double x_new_ = a_point.getX() + trans_x; |
|---|
| 53 | double y_new_ = a_point.getY() + trans_y; |
|---|
| 54 | double z_new_ = a_point.getZ() + trans_z; |
|---|
| 55 | |
|---|
| 56 | a_point.set(x_new_, y_new_, z_new_); |
|---|
| 57 | } |
|---|
| 58 | |
|---|
| 59 | void RotateMatrix(const vector<double> &rotmatrix, Point3D &a_point) |
|---|
| 60 | { |
|---|
| 61 | if (rotmatrix.size() != 9) |
|---|
| 62 | throw std::runtime_error("The size for rotation matrix vector has to be 9."); |
|---|
| 63 | |
|---|
| 64 | double xold = a_point.getX(); |
|---|
| 65 | double yold = a_point.getY(); |
|---|
| 66 | double zold = a_point.getZ(); |
|---|
| 67 | |
|---|
| 68 | double x_new_ = rotmatrix[0]*xold + rotmatrix[1]*yold + rotmatrix[2]*zold; |
|---|
| 69 | double y_new_ = rotmatrix[3]*xold + rotmatrix[4]*yold + rotmatrix[5]*zold; |
|---|
| 70 | double z_new_ = rotmatrix[6]*xold + rotmatrix[7]*yold + rotmatrix[8]*zold; |
|---|
| 71 | |
|---|
| 72 | a_point.set(x_new_, y_new_, z_new_); |
|---|
| 73 | } |
|---|
