|
TuttleOFX
1
|
00001 #include "MatrixManipulation.hpp" 00002 #include "ofxCore.h" 00003 00004 namespace tuttle { 00005 namespace plugin { 00006 namespace colorCubeViewer { 00007 00008 /* 00009 * Matrix product with a 3D vector and a 4*4 matrix 00010 */ 00011 void productVectorMatrix(const Vect4& v, const Matrix4& m, Vect4& result) 00012 { 00013 result[0] = m[0]*v[0]+m[4]*v[1]+m[8]*v[2]+m[12]*1; //compute X value 00014 result[1] = m[1]*v[0]+m[5]*v[1]+m[9]*v[2]+m[13]*1; //compute Y value 00015 result[2] = m[2]*v[0]+m[6]*v[1]+m[10]*v[2]+m[14]*1; //compute Z value 00016 result[3] = m[3]*v[0]+m[7]*v[1]+m[11]*v[2]+m[15]*1; //compute w 00017 } 00018 00019 00020 /* 00021 * Does the multiplication A=A*B : all the matrices are described column-major 00022 */ 00023 void multMatrixBtoMatrixA( Matrix4& A, const Matrix4& B) 00024 { 00025 unsigned int i=0; // row index 00026 unsigned int j=0; // column index 00027 Matrix4 temp; 00028 00029 for (unsigned int iValue=0 ; iValue<16 ; ++iValue) 00030 { 00031 temp[iValue]=0; 00032 i=iValue%4; // cause column-major 00033 j=iValue/4; // cause column-major 00034 for (unsigned int k=0 ; k<4 ; ++k) 00035 { 00036 int indexik=k*4+i; 00037 int indexkj=j*4+k; 00038 temp[iValue]+=A[indexik]*B[indexkj]; 00039 } 00040 } 00041 //recopy temporary matrix into result 00042 A = temp; 00043 } 00044 00045 /* 00046 * Sets the provided matrix to identity 00047 */ 00048 void setToIdentity(Matrix4& matrix) 00049 { 00050 Matrix4 Id = { { 1.0, 0.0, 0.0, 0.0, 00051 0.0, 1.0, 0.0, 0.0, 00052 0.0, 0.0, 1.0, 0.0, 00053 0.0, 0.0, 0.0, 1.0 } }; 00054 //recopy matrix 00055 matrix = Id; 00056 } 00057 00058 /* 00059 * Sets the provided matrix to a rotate matrix of angle "angle", around axis "axis" 00060 */ 00061 void setToRotate( Matrix4& matrix, const double& angle, const Axis axis ) 00062 { 00063 const double c=cos(angle); 00064 const double s=sin(angle); 00065 const double x=axis[0]; 00066 const double y=axis[1]; 00067 const double z=axis[2]; 00068 00069 if ((x==1.0) && (y==0.0) && (z==0.0)) //current axis is X 00070 { 00071 Matrix4 R = { { 1.0, 0.0, 0.0, 0.0, 00072 0.0, c, s, 0.0, 00073 0.0, -s, c, 0.0, 00074 0.0, 0.0, 0.0, 1.0 } }; 00075 //recopy matrix 00076 matrix = R; 00077 } 00078 else 00079 { 00080 if ((x==0.0) && (y==1.0) && (z==0.0)) //current axis is Y 00081 { 00082 Matrix4 R = { { c, 0.0, -s, 0.0, 00083 0.0, 1.0, 0.0, 0.0, 00084 s, 0.0, c, 0.0, 00085 0.0, 0.0, 0.0, 1.0 } }; 00086 //recopy matrix 00087 matrix = R; 00088 } 00089 else 00090 { 00091 00092 if ((x==0.0) && (y==0.0) && (z==1.0)) //current axis is Z 00093 { 00094 Matrix4 R = { { c, s, 0.0, 0.0, 00095 -s, c, 0.0, 0.0, 00096 0.0, 0.0, 1.0, 0.0, 00097 0.0, 0.0, 0.0, 1.0 } }; 00098 //recopy matrix 00099 matrix = R; 00100 } 00101 else //Rotation on non standard axis 00102 { 00103 Matrix4 R = { { (1.0-c)*(x*x-1.0) + 1.0, (1.0-c)*x*y + (z*s), (1.0-c)*x*z - (y*s), 0.0, 00104 (1.0-c)*x*y - (z*s), (1.0-c)*(y*y-1.0) + 1.0, (1.0-c)*y*z + (x*s), 0.0, 00105 (1.0-c)*x*z + (y*s), (1.0-c)*y*z - (x*s), (1.0-c)*(z*z-1.0) + 1.0, 0.0, 00106 0.0, 0.0, 0.0, 1.0 } }; 00107 //recopy matrix 00108 matrix=R; 00109 } 00110 } 00111 } 00112 } 00113 00114 /* 00115 *Sets the provided matrix to a translate matrix on vector t 00116 */ 00117 void setToTranslate(Matrix4& matrix, const Vect4& t) 00118 { 00119 Matrix4 T = { { 1.0, 0.0, 0.0, 0.0, 00120 0.0, 1.0, 0.0, 0.0, 00121 0.0, 0.0, 1.0, 0.0, 00122 t[0], t[1], t[2], 1.0 } }; 00123 //recopy matrix 00124 matrix=T; 00125 } 00126 00127 /* 00128 * Create rotation matrix with rotation center (translate + rotates + inverse translate) 00129 */ 00130 Matrix4 constructRotationMatrix(const Ofx3DPointD& rotationCenter, const double& angleX, const double& angleY, const double& angleZ) 00131 { 00132 //define axes 00133 Axis axisX = { { 1,0,0 } }; //define axe X 00134 Axis axisY = { { 0,1,0 } }; //define axe Y 00135 Axis axisZ = { { 0,0,1 } }; //define axe Z 00136 00137 //Define and create rotation + translation matrix 00138 Matrix4 rotationTranslationMatrix; //initialize 00139 setToIdentity(rotationTranslationMatrix); //set matrix center to identity 00140 00141 //Define and construct rotation matrices 00142 Matrix4 rotationMatrixX; //initialize rotation matrix X 00143 setToRotate(rotationMatrixX, angleX/10.0, axisX); //construct rotation matrix X 00144 Matrix4 rotationMatrixY; //initialize rotation matrix Y 00145 setToRotate(rotationMatrixY, angleY/10.0, axisY); //construct rotation matrix Y 00146 Matrix4 rotationMatrixZ; //initialize rotation matrix Z 00147 setToRotate(rotationMatrixZ, angleZ/10.0, axisZ); //construct rotation matrix Z 00148 00149 //Define and construct translation matrix 00150 Matrix4 translationMatrix; //initialize translation matrix 00151 //Construct translation vector 00152 Vect4 translationVector = { { rotationCenter.x, rotationCenter.y, rotationCenter.z, 1.0 } }; 00153 setToTranslate(translationMatrix,translationVector); //construct translation matrix 00154 00155 //Define and construct inverse translation matrix 00156 Matrix4 translationInverseMatrix; //initialize inverse translation matrix 00157 //Construct inverse translation vector 00158 Vect4 inverseTranslationVector = { { -rotationCenter.x, -rotationCenter.y, -rotationCenter.z, 1.0 } }; 00159 setToTranslate(translationInverseMatrix,inverseTranslationVector); //construct inverse translation matrix 00160 00161 //Construct final reference center matrix (inverse order of application) 00162 multMatrixBtoMatrixA(rotationTranslationMatrix, translationMatrix); //translation matrix 00163 multMatrixBtoMatrixA(rotationTranslationMatrix, rotationMatrixZ); //rotation Z axis 00164 multMatrixBtoMatrixA(rotationTranslationMatrix, rotationMatrixY); //rotation Y axis 00165 multMatrixBtoMatrixA(rotationTranslationMatrix, rotationMatrixX); //rotation X axis 00166 multMatrixBtoMatrixA(rotationTranslationMatrix, translationInverseMatrix); //inverse translation matrix 00167 //return result 00168 return rotationTranslationMatrix; 00169 } 00170 00171 00172 } 00173 } 00174 }
1.7.5.1 
