Rotação um dia fica bom :-)

olha só o que era pra ser uma rotação em torno do eixo Z, mas hj vou fazer uma enorme bateria de teste pra ver o q dá, em quanto isso tai o código c vc puder me ajudar

#include
#include "math.h"
#include "SDL.h"
#include "SDL_gfxPrimitives.h"
#define deg2rad M_PI/180.0
#define HALFX 320
#define HALFY 240
#define RESX 640
#define RESY 480
class Vertex
{
public:
float x;
float y;
float z;
const Vertex& operator= (Vertex& V)
{
x = V.x;
y = V.y;
z = V.z;
return *this;
};
};
Vertex V1,V2;
/* translacao */
void VecTranslate(Vertex *V,float tx,float ty,float tz)
{
V->x += tx;
V->y += ty;
V->z += tz;
}
/* escala */
void VecScale(Vertex *V,float sx,float sy,float sz)
{
V->x *= sx;
V->y *= sy;
V->z *= sz;
}
/* rotacao em eixo arbitrario */
void VecRotate(Vertex *V,float x,float y,float z,float theta)
{
float xx,yy,zz,xy,xz,yz,xSintheta,ySintheta,zSintheta,Costheta,Sintheta,UmMenosCostheta;
Costheta = cosf(theta);
Sintheta = sinf(theta);
UmMenosCostheta = 1-Costheta;
xx = x*x*UmMenosCostheta;
yy = y*y*UmMenosCostheta;
zz = z*z*UmMenosCostheta;
xy = z*y*UmMenosCostheta;
xz = x*z*UmMenosCostheta;
yz = y*z*UmMenosCostheta;
xSintheta = x*Sintheta;
ySintheta = y*Sintheta;
zSintheta = z*Sintheta;
V->x = (xx+Costheta) * V->x + (xy - zSintheta) * V->y + (xz + ySintheta) * V->z;
V->y = (xy+zSintheta) * V->x + (yy + Costheta) * V->y + (yz - xSintheta) * V->z;
V->z = (xz-ySintheta) * V->x + (yz + xSintheta) * V->y + (zz + Costheta) * V->z;
}
/* Projecao */
void VecProject(Vertex *V,float distance)
{
float x,y;
x = V->x / V->z;
y = V->y / V->z;
V->x = HALFX +(RESX*x);
V->y = HALFY +(RESY*y);
}
int main()
{
Vertex V1,V2,Va,Vb;
SDL_Surface *screen;
SDL_Init(SDL_INIT_VIDEO);
screen = SDL_SetVideoMode(RESX,RESY,8,SDL_SWSURFACE);
if (screen == NULL)
{
printf("Nao foi possivel iniciar o video\n");
exit(0);
}
V1.x = 5;
V1.y = 5;
V1.z = 10;
V2.x = 15;
V2.y = 5;
V2.z = 10;
VecTranslate(&V1,0,0,50);
VecTranslate(&V2,0,0,50);
VecScale(&V1,1.5,1.5,1.5);
VecScale(&V2,1.5,1.5,1.5);
float rx =1*deg2rad;
while (1)
{
if (rx >= 360*deg2rad)
rx = 1*deg2rad;
Va = V1;
Vb = V2;
VecRotate(&Va,0,0,1,rx);
VecRotate(&Vb,0,0,1,rx);
VecProject(&Va,100);
VecProject(&Vb,100);
lineColor(screen,Va.x,Va.y,Vb.x,Vb.y,0xFFFFFFFF);
SDL_UpdateRect(screen,0,0,RESX,RESY);
rx+=(1*deg2rad);
}
return 0;
}