1 /********************************************************* 2 / MANDELBULBER 3 / Vector and matrix algebra 4 / Functions for CMatrix33 and CRotationMatrix classes 5 / 6 / author: Krzysztof Marczak 7 / contact: buddhi1980@gmail.com 8 / licence: GNU GPL v3.0 9 / 10 ********************************************************/ 11 12 #include "algebra.hpp" 13 14 /***************** class CMatrix33 ***********************/ CMatrix33()15CMatrix33::CMatrix33() 16 { 17 m11 = 0; 18 m12 = 0; 19 m13 = 0; 20 m21 = 0; 21 m22 = 0; 22 m23 = 0; 23 m31 = 0; 24 m32 = 0; 25 m33 = 0; 26 } 27 CMatrix33(const CMatrix33 & matrix)28CMatrix33::CMatrix33(const CMatrix33 &matrix) 29 { 30 m11 = matrix.m11; 31 m12 = matrix.m12; 32 m13 = matrix.m13; 33 m21 = matrix.m21; 34 m22 = matrix.m22; 35 m23 = matrix.m23; 36 m31 = matrix.m31; 37 m32 = matrix.m32; 38 m33 = matrix.m33; 39 } 40 operator =(const CMatrix33 & matrix)41CMatrix33& CMatrix33::operator=(const CMatrix33 &matrix) 42 { 43 m11 = matrix.m11; 44 m12 = matrix.m12; 45 m13 = matrix.m13; 46 m21 = matrix.m21; 47 m22 = matrix.m22; 48 m23 = matrix.m23; 49 m31 = matrix.m31; 50 m32 = matrix.m32; 51 m33 = matrix.m33; 52 return *this; 53 } 54 operator *(const CMatrix33 & matrix) const55CMatrix33 CMatrix33::operator*(const CMatrix33 &matrix) const 56 { 57 CMatrix33 result; 58 result.m11 = m11 * matrix.m11 + m12 * matrix.m21 + m13 * matrix.m31; 59 result.m12 = m11 * matrix.m12 + m12 * matrix.m22 + m13 * matrix.m32; 60 result.m13 = m11 * matrix.m13 + m12 * matrix.m23 + m13 * matrix.m33; 61 result.m21 = m21 * matrix.m11 + m22 * matrix.m21 + m23 * matrix.m31; 62 result.m22 = m21 * matrix.m12 + m22 * matrix.m22 + m23 * matrix.m32; 63 result.m23 = m21 * matrix.m13 + m22 * matrix.m23 + m23 * matrix.m33; 64 result.m31 = m31 * matrix.m11 + m32 * matrix.m21 + m33 * matrix.m31; 65 result.m32 = m31 * matrix.m12 + m32 * matrix.m22 + m33 * matrix.m32; 66 result.m33 = m31 * matrix.m13 + m32 * matrix.m23 + m33 * matrix.m33; 67 return result; 68 } 69 operator *(const CVector3 & vector) const70CVector3 CMatrix33::operator*(const CVector3 &vector) const 71 { 72 CVector3 result; 73 result.x = m11 * vector.x + m12 * vector.y + m13 * vector.z; 74 result.y = m21 * vector.x + m22 * vector.y + m23 * vector.z; 75 result.z = m31 * vector.x + m32 * vector.y + m33 * vector.z; 76 return result; 77 } 78 79 /**************** class RotarionMatrix **********************/ CRotationMatrix()80CRotationMatrix::CRotationMatrix() 81 { 82 matrix.m11 = 1.0; 83 matrix.m12 = 0.0; 84 matrix.m13 = 0.0; 85 matrix.m21 = 0.0; 86 matrix.m22 = 1.0; 87 matrix.m23 = 0.0; 88 matrix.m31 = 0.0; 89 matrix.m32 = 0.0; 90 matrix.m33 = 1.0; 91 zero = true; 92 } 93 RotateX(double angle)94void CRotationMatrix::RotateX(double angle) 95 { 96 if (angle != 0.0) 97 { 98 CMatrix33 rot; 99 double s = sin(angle); 100 double c = cos(angle); 101 rot.m11 = 1.0; 102 rot.m22 = c; 103 rot.m33 = c; 104 rot.m23 = -s; 105 rot.m32 = s; 106 matrix = matrix * rot; 107 zero = false; 108 } 109 } 110 RotateY(double angle)111void CRotationMatrix::RotateY(double angle) 112 { 113 if (angle != 0.0) 114 { 115 CMatrix33 rot; 116 double s = sin(angle); 117 double c = cos(angle); 118 rot.m22 = 1.0; 119 rot.m33 = c; 120 rot.m11 = c; 121 rot.m31 = -s; 122 rot.m13 = s; 123 matrix = matrix * rot; 124 zero = false; 125 } 126 } 127 RotateZ(double angle)128void CRotationMatrix::RotateZ(double angle) 129 { 130 if (angle != 0.0) 131 { 132 CMatrix33 rot; 133 double s = sin(angle); 134 double c = cos(angle); 135 rot.m33 = 1.0; 136 rot.m11 = c; 137 rot.m22 = c; 138 rot.m12 = -s; 139 rot.m21 = s; 140 matrix = matrix * rot; 141 zero = false; 142 } 143 } 144 SetRotation(double angles[3])145void CRotationMatrix::SetRotation(double angles[3]) 146 { 147 Null(); 148 RotateZ(angles[2]); 149 RotateY(angles[1]); 150 RotateX(angles[0]); 151 } 152 SetRotation(double alfa,double beta,double gamma)153void CRotationMatrix::SetRotation(double alfa, double beta, double gamma) 154 { 155 Null(); 156 RotateZ(alfa); 157 RotateY(beta); 158 RotateX(gamma); 159 } 160 RotateVector(const CVector3 & vector) const161CVector3 CRotationMatrix::RotateVector(const CVector3& vector) const 162 { 163 if (!zero) 164 { 165 CVector3 vector2 = matrix * vector; 166 return vector2; 167 } 168 else 169 { 170 return vector; 171 } 172 } 173 Null()174void CRotationMatrix::Null() 175 { 176 //CRotationMatrix(); 177 matrix.m11 = 1.0; 178 matrix.m12 = 0.0; 179 matrix.m13 = 0.0; 180 matrix.m21 = 0.0; 181 matrix.m22 = 1.0; 182 matrix.m23 = 0.0; 183 matrix.m31 = 0.0; 184 matrix.m32 = 0.0; 185 matrix.m33 = 1.0; 186 zero = true; 187 } 188 GetAlfa() const189double CRotationMatrix::GetAlfa() const 190 { 191 return atan2(matrix.m12,matrix.m22); 192 } 193 GetBeta() const194double CRotationMatrix::GetBeta() const 195 { 196 return asin(-matrix.m32); 197 } 198 GetGamma() const199double CRotationMatrix::GetGamma() const 200 { 201 return atan2(matrix.m31,matrix.m33); 202 } 203 Transpose() const204CRotationMatrix CRotationMatrix::Transpose() const 205 { 206 CRotationMatrix m; 207 m.matrix.m11 = matrix.m11; 208 m.matrix.m12 = matrix.m21; 209 m.matrix.m13 = matrix.m31; 210 m.matrix.m21 = matrix.m12; 211 m.matrix.m22 = matrix.m22; 212 m.matrix.m23 = matrix.m32; 213 m.matrix.m31 = matrix.m13; 214 m.matrix.m32 = matrix.m23; 215 m.matrix.m33 = matrix.m33; 216 m.zero = false; 217 return m; 218 } 219