#include "stdafx.h"
#include <conio.h>
#include <math.h>
struct vector_axis
{
double x, y, z;
};
void vector_cross_product(vector_axis v1,vector_axis v2,vector_axis &result)
{
result.x = v1.y * v2.z - v1.z * v2.y;
result.y = v1.z * v2.x - v1.x * v2.z;
result.z = v1.x * v2.y - v1.y * v2.x;
}
void vector_normalize(vector_axis &v)
{
double k = sqrt((v.x*v.x + v.y*v.y + v.z*v.z));
v.x /= k; v.y /= k; v.z /= k;
}
void GetOCStoWCSMatrix(vector_axis norm, vector_axis &Ax, vector_axis &Ay, vector_axis &Az)
{
vector_axis Wy,Wz;
Wy.x = 0.; Wy.y = 1.; Wy.z = 0.;
Wz.x = 0.; Wz.y = 0.; Wz.z = 1.;
if ((fabs(norm.x)<1/64.) && (fabs(norm.y)<1/64.))
{
vector_cross_product(Wy,norm,Ax);
}
else
{
vector_cross_product(Wz,norm,Ax);
}
vector_normalize(Ax);
vector_cross_product(norm,Ax,Ay);
vector_normalize(Ay);
Az = norm;
}
void TransWithMatrix(vector_axis pIn, vector_axis Ax, vector_axis Ay, vector_axis Az, vector_axis &pOut)
{
pOut.x = pIn.x * Ax.x + pIn.y * Ay.x + pIn.z * Az.x;
pOut.y = pIn.x * Ax.y + pIn.y * Ay.y + pIn.z * Az.y;
pOut.z = pIn.x * Ax.z + pIn.y * Ay.z + pIn.z * Az.z;
}
int _tmain(int argc, _TCHAR* argv[])
{
vector_axis norm, Ax, Ay, Az;
norm.x = -0.6446022880805341;
norm.y = -0.4835076803238965;
norm.z = 0.5922062252873951;
GetOCStoWCSMatrix(norm,Ax,Ay,Az);
vector_axis p;
p.x = 103.3406489926814; p.y = 96.54382562220856; p.z = 10;
TransWithMatrix(p, Ax, Ay, Az, p);
_tprintf (_T("\nx=%f y=%f z=%f"), p.x, p.y, p.z);
p.x = 206.6812979853627; p.y = 193.0876512444171; p.z = 10;
TransWithMatrix(p, Ax, Ay, Az, p);
_tprintf (_T("\nx=%f y=%f z=%f"), p.x, p.y, p.z);
_getch();
return 0;
}