run_convert.c

00001 /*
00002   イセア鮟サ
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 
00007 
00008 VXV::Matrix4D VXV::createConvertMatrix(const Position3D pos) {
00009   Matrix4D conv;
00010 
00011   conv.a11 = cos(pos.yt.to_rad()) * cos(pos.zt.to_rad());
00012   conv.a21 = -cos(pos.xt.to_rad())*sin(pos.zt.to_rad())
00013     + sin(pos.xt.to_rad())*sin(pos.yt.to_rad())*cos(pos.zt.to_rad());
00014   conv.a31 = cos(pos.xt.to_rad())*sin(pos.yt.to_rad())*cos(pos.zt.to_rad())
00015     + sin(pos.xt.to_rad())*sin(pos.zt.to_rad());
00016   conv.a41 = pos.x;
00017 
00018   conv.a12 = cos(pos.yt.to_rad())*sin(pos.zt.to_rad());
00019   conv.a22 = cos(pos.xt.to_rad())*cos(pos.zt.to_rad())
00020     + sin(pos.xt.to_rad())*sin(pos.yt.to_rad())*sin(pos.zt.to_rad());
00021   conv.a32 = cos(pos.xt.to_rad())*sin(pos.yt.to_rad())*sin(pos.zt.to_rad())
00022     - sin(pos.xt.to_rad())*cos(pos.zt.to_rad());
00023   conv.a42 = pos.y;
00024 
00025   conv.a13 = -sin(pos.yt.to_rad());
00026   conv.a23 = sin(pos.xt.to_rad())*cos(pos.yt.to_rad());
00027   conv.a33 = cos(pos.xt.to_rad())*cos(pos.yt.to_rad());
00028   conv.a43 = pos.z;
00029 
00030   return conv;
00031 }
00032 
00033 
00034 void VXV::convert(Position3D& converted, const Position3D& input,
00035                   const Matrix4D& matrix) {
00036   Position3D pos;
00037   pos.x = (int)(input.x * matrix.a11 +
00038                       input.y * matrix.a21 +
00039                       input.z * matrix.a31) + matrix.a41;
00040   pos.y = (int)(input.x * matrix.a12 +
00041                       input.y * matrix.a22 +
00042                       input.z * matrix.a32) + matrix.a42;
00043   pos.z = (int)(input.x * matrix.a13 +
00044                       input.y * matrix.a23 +
00045                       input.z * matrix.a33) + matrix.a43;
00046   converted.x = pos.x;
00047   converted.y = pos.y;
00048   converted.z = pos.z;
00049 }
00050