Object3D.cpp

00001 /* 
00002  * libmbed-graphics 2D and wireframe 3D graphics library for the MBED
00003  * microcontroller platform
00004  * Copyright (C) <2009> Michael Sheldon <mike@mikeasoft.com>
00005  *
00006  * This library is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU Library General Public
00008  * License as published by the Free Software Foundation; either
00009  * version 2 of the License, or (at your option) any later version.
00010  *
00011  * This library is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Library General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Library General Public
00017  * License along with this library; if not, write to the
00018  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
00019  * Boston, MA 02111-1307, USA.
00020  */
00021  
00022 #include "Object3D.h"
00023 
00024 Object3D::Object3D() {
00025     _x = 0;
00026     _y = 0;
00027     _z = 0;
00028     _rx = 0;
00029     _ry = 0;
00030     _rz = 0;
00031     _sx = 1;
00032     _sy = 1;
00033     _sz = 1;
00034     _colour = 0xffffff;
00035 }
00036 
00037 void Object3D::position(int x, int y, int z) {
00038     _x = x;
00039     _y = y;
00040     _z = z;
00041 }
00042 
00043 void Object3D::rotate(float rx, float ry, float rz) {
00044     _rx = rx;
00045     _ry = ry;
00046     _rz = rz;
00047 }
00048 
00049 void Object3D::scale(float sx, float sy, float sz) {
00050     _sx = sx;
00051     _sy = sy;
00052     _sz = sz;
00053 }
00054 
00055 void Object3D::colour(int colour) {
00056     _colour = colour;
00057 }
00058 
00059 #define FASTSINCOS
00060 
00061 #ifdef FASTSINCOS
00062 
00063 // http://www.dspguru.com/dsp/tricks/parabolic-approximation-of-sin-and-cos
00064 
00065 #define M_PI       3.14159265358979323846
00066 #define M_PI_2     1.57079632679489661923
00067 #define M_PI_4     0.785398163397448309616
00068 #define M_C        0.71256755058
00069 
00070 void sincosf(float angle, float *sinx, float *cosx)
00071 {
00072     int quarter = int(floor(angle/M_PI_2));
00073     
00074     float modphase = angle - quarter*M_PI_2;
00075     if ( modphase < 0 )
00076         modphase += M_PI_2;
00077     
00078     quarter %= 4;
00079     if ( quarter < 0 )
00080         quarter += 4;
00081     
00082     modphase = modphase/M_PI_2;
00083 
00084     //printf("Angle: %f, q: %d, mod: %f\n", angle, quarter, modphase);
00085     float x, temp;
00086     switch (quarter)
00087     { 
00088     case 0:
00089         // First quarter, angle = 0 .. pi/2
00090         x = modphase - 0.5f;      // 1 sub
00091         temp = (2 - 4*M_C)*x*x + M_C; // 2 mul, 1 add
00092         *sinx = temp + x;              // 1 add
00093         *cosx = temp - x;              // 1 sub
00094         break;
00095     case 1:
00096         // Second quarter, angle = pi/2 .. pi
00097         x = 0.5f - modphase;      // 1 sub
00098         temp = (2 - 4*M_C)*x*x + M_C; // 2 mul, 1 add
00099         *sinx = x + temp;              // 1 add
00100         *cosx = x - temp;              // 1 sub
00101         break;
00102     case 2:
00103         // Third quarter, angle = pi .. 1.5pi
00104         x = modphase - 0.5f;      // 1 sub
00105         temp = (4*M_C - 2)*x*x - M_C; // 2 mul, 1 sub
00106         *sinx = temp - x;              // 1 sub
00107         *cosx = temp + x;              // 1 add
00108         break;
00109     case 3:
00110         // Fourth quarter, angle = 1.5pi..2pi
00111         x = modphase - 0.5f;      // 1 sub
00112         temp = (2 - 4*M_C)*x*x + M_C; // 2 mul, 1 add
00113         *sinx = x - temp;              // 1 sub
00114         *cosx = x + temp;              // 1 add
00115         break;
00116     }
00117 }
00118 
00119 #else // FASTSINCOS
00120 
00121 void sincosf(float angle, float *sinx, float *cosx)
00122 {
00123   *sinx = sin(angle);
00124   *cosx = cos(angle);
00125 }
00126 
00127 #endif // FASTSINCOS
00128 
00129 
00130 void rotate3d(int *x, int *y, int *z, float anglex, float angley, float anglez, int count)
00131 {    
00132     float sinx, cosx, siny, cosy, sinz, cosz;
00133     sincosf(anglex, &sinx, &cosx);    
00134     sincosf(angley, &siny, &cosy);
00135     sincosf(anglez, &sinz, &cosz);
00136     
00137     for ( int i=0; i < count; i++)
00138     {
00139         int x1 = x[i];
00140         int y1 = y[i];
00141         int z1 = z[i];
00142         
00143         int y2 = cosx * y1 - sinx * z1;
00144         int z2 = sinx * y1 + cosx * z1;
00145     
00146         int x2 = cosy * x1 + siny * z2;
00147         int z3 = -siny * x1 + cosy * z2;
00148         
00149         int x3 = cosz * x2 - sinz * y2;
00150         int y3 = sinz * x2 + cosz * y2;
00151         
00152         x[i] = x3;
00153         y[i] = y3;
00154         z[i] = z3;
00155     }
00156 }