/********************************************************************************** Snes9x - Portable Super Nintendo Entertainment System (TM) emulator. (c) Copyright 1996 - 2002 Gary Henderson (gary.henderson@ntlworld.com), Jerremy Koot (jkoot@snes9x.com) (c) Copyright 2002 - 2004 Matthew Kendora (c) Copyright 2002 - 2005 Peter Bortas (peter@bortas.org) (c) Copyright 2004 - 2005 Joel Yliluoma (http://iki.fi/bisqwit/) (c) Copyright 2001 - 2006 John Weidman (jweidman@slip.net) (c) Copyright 2002 - 2006 funkyass (funkyass@spam.shaw.ca), Kris Bleakley (codeviolation@hotmail.com) (c) Copyright 2002 - 2007 Brad Jorsch (anomie@users.sourceforge.net), Nach (n-a-c-h@users.sourceforge.net), zones (kasumitokoduck@yahoo.com) (c) Copyright 2006 - 2007 nitsuja BS-X C emulator code (c) Copyright 2005 - 2006 Dreamer Nom, zones C4 x86 assembler and some C emulation code (c) Copyright 2000 - 2003 _Demo_ (_demo_@zsnes.com), Nach, zsKnight (zsknight@zsnes.com) C4 C++ code (c) Copyright 2003 - 2006 Brad Jorsch, Nach DSP-1 emulator code (c) Copyright 1998 - 2006 _Demo_, Andreas Naive (andreasnaive@gmail.com) Gary Henderson, Ivar (ivar@snes9x.com), John Weidman, Kris Bleakley, Matthew Kendora, Nach, neviksti (neviksti@hotmail.com) DSP-2 emulator code (c) Copyright 2003 John Weidman, Kris Bleakley, Lord Nightmare (lord_nightmare@users.sourceforge.net), Matthew Kendora, neviksti DSP-3 emulator code (c) Copyright 2003 - 2006 John Weidman, Kris Bleakley, Lancer, z80 gaiden DSP-4 emulator code (c) Copyright 2004 - 2006 Dreamer Nom, John Weidman, Kris Bleakley, Nach, z80 gaiden OBC1 emulator code (c) Copyright 2001 - 2004 zsKnight, pagefault (pagefault@zsnes.com), Kris Bleakley, Ported from x86 assembler to C by sanmaiwashi SPC7110 and RTC C++ emulator code (c) Copyright 2002 Matthew Kendora with research by zsKnight, John Weidman, Dark Force S-DD1 C emulator code (c) Copyright 2003 Brad Jorsch with research by Andreas Naive, John Weidman S-RTC C emulator code (c) Copyright 2001-2006 byuu, John Weidman ST010 C++ emulator code (c) Copyright 2003 Feather, John Weidman, Kris Bleakley, Matthew Kendora Super FX x86 assembler emulator code (c) Copyright 1998 - 2003 _Demo_, pagefault, zsKnight, Super FX C emulator code (c) Copyright 1997 - 1999 Ivar, Gary Henderson, John Weidman Sound DSP emulator code is derived from SNEeSe and OpenSPC: (c) Copyright 1998 - 2003 Brad Martin (c) Copyright 1998 - 2006 Charles Bilyue' SH assembler code partly based on x86 assembler code (c) Copyright 2002 - 2004 Marcus Comstedt (marcus@mc.pp.se) 2xSaI filter (c) Copyright 1999 - 2001 Derek Liauw Kie Fa HQ2x, HQ3x, HQ4x filters (c) Copyright 2003 Maxim Stepin (maxim@hiend3d.com) Win32 GUI code (c) Copyright 2003 - 2006 blip, funkyass, Matthew Kendora, Nach, nitsuja Mac OS GUI code (c) Copyright 1998 - 2001 John Stiles (c) Copyright 2001 - 2007 zones Specific ports contains the works of other authors. See headers in individual files. Snes9x homepage: http://www.snes9x.com Permission to use, copy, modify and/or distribute Snes9x in both binary and source form, for non-commercial purposes, is hereby granted without fee, providing that this license information and copyright notice appear with all copies and any derived work. This software is provided 'as-is', without any express or implied warranty. In no event shall the authors be held liable for any damages arising from the use of this software or it's derivatives. Snes9x is freeware for PERSONAL USE only. Commercial users should seek permission of the copyright holders first. Commercial use includes, but is not limited to, charging money for Snes9x or software derived from Snes9x, including Snes9x or derivatives in commercial game bundles, and/or using Snes9x as a promotion for your commercial product. The copyright holders request that bug fixes and improvements to the code should be forwarded to them so everyone can benefit from the modifications in future versions. Super NES and Super Nintendo Entertainment System are trademarks of Nintendo Co., Limited and its subsidiary companies. **********************************************************************************/ /**************************************************************** * * * AuxMath.cpp - Galeria de funciones auxiliares geom�ricas * * Implementaci�. * * * * By Cuervo (1999) * * diego_tartara@ciudad.com.ar * ****************************************************************/ //headers #include "auxmath.h" #ifdef _MSC_VER #pragma warning (disable : 4244) //Conversi� de double a float #endif //----------------------------------------------------------------------/ ////////////////////////////////////////////////////// //Implementaci� de los operadores de la clase vect // ////////////////////////////////////////////////////// ////////////////// //Constructores // ////////////////// vect::vect(const float x1, const float y1, const float z1) { x = x1; y = y1; z = z1; } vect::vect(const vect& v) { x = v.x; y = v.y; z = v.z; } ///////////////////////////// //Operadores de asignaci� // ///////////////////////////// vect& vect::operator = (const vect v) { x = v.x; y = v.y; z = v.z; return *this; } vect& vect::operator += (const vect v) { x += v.x; y += v.y; z += v.z; return *this; } vect& vect::operator -= (const vect v) { x -= v.x; y -= v.y; z -= v.z; return *this; } vect& vect::operator *= (const float num) { x *= num; y *= num; z *= num; return *this; } vect& vect::operator *= (const vect v) { x *= v.x; y *= v.y; z *= v.z; return *this; } vect& vect::operator /= (const float num) { x /= num; y /= num; z /= num; return *this; } vect& vect::operator /= (const vect v) { x /= v.x; y /= v.y; z /= v.z; return *this; } ///////////////////////////////// // Operador de acceso indexado // ///////////////////////////////// const float& vect::operator[](int i) const { return n[i]; } //////////////////////////////////////////////////////////////// // / // Implementaciones de operadores friends para la clase vect / // / //////////////////////////////////////////////////////////////// ///////////////////////// // Operadores binarios // ///////////////////////// vect operator+ (const vect& vect1, const vect& vect2) { vect result ( vect1.x + vect2.x, vect1.y + vect2.y, vect1.z + vect2.z ); return result; } vect operator- (const vect& vect1, const vect& vect2) { vect result ( vect1.x - vect2.x, vect1.y - vect2.y, vect1.z - vect2.z ); return result; } vect operator* (const vect& vect1,const vect& vect2) { vect result ( vect1.x * vect2.x, vect1.y * vect2.y, vect1.z * vect2.z ); return result; } vect operator* (const vect& vect1, const float num) { vect result ( num * vect1.x, num * vect1.y, num * vect1.z ); return result; } vect operator* (const float num, const vect& vect1) { vect result ( num * vect1.x, num * vect1.y, num * vect1.z ); return result; } vect operator/ (const vect& vect1, const vect& vect2) { vect result ( vect1.x / vect2.x, vect1.y / vect2.y, vect1.z / vect2.z); return result; } vect operator/ (const vect& vect1, const float num) { vect result ( vect1.x / num, vect1.y / num, vect1.z / num ); return result; } ////////////////////////// // Operadores unarios // ////////////////////////// vect operator- (const vect& vect1) { vect result ( -vect1.x, -vect1.y, -vect1.z); return result; } /////////////////////////////// // Operadores de comparaci� // /////////////////////////////// int operator> (const vect& vect1, const vect& vect2) { return Mod(vect1) > Mod(vect2); } int operator>= (const vect& vect1, const vect& vect2) { return Mod(vect1) >= Mod(vect2); } int operator< (const vect& vect1, const vect& vect2) { return Mod(vect1) < Mod(vect2); } int operator<= (const vect& vect1, const vect& vect2) { return Mod(vect1) >= Mod(vect2); } int operator== (const vect& vect1,const vect& vect2) { return (vect1.x == vect2.x && vect1.y == vect2.y && vect1.z == vect2.z); } int operator!= (const vect& vect1, const vect& vect2) { return !(vect1.x == vect2.x && vect1.y == vect2.y && vect1.z == vect2.z); } void vect::Normalize(void) { float mod = GetMod(); x /= mod; y /= mod; z /= mod; } //a partir de dos vectores, genera el ortonormal (perpendicular) //a ambos para completar una base void vect::MakeOrthonormal(vect &base1, vect &base2) { base1.Normalize(); base2.Normalize(); *this = VectXProd(base1,base2); Normalize(); base1 = VectXProd(base2,*this); base1.Normalize(); } //----------------------------------------------------------------------/ ////////////////////////////////////////////////////// //Implementaci� de los operadores de la clase quat // ////////////////////////////////////////////////////// ////////////////// //Constructores // ////////////////// quat::quat(const quat& q) { x = q.x; y = q.y; z = q.z; w = q.w; } quat::quat(const float x1, const float y1, const float z1, const float angle) { x = x1; y = y1; z = z1; w = angle; } inline quat::quat(const vect& v, const float angle) { x = v.x; y = v.y; z = v.z; w = angle; } quat::quat(const float yaw, const float pitch, const float roll) { //el orden de las rotaciones es: //r' = roll ( pitch ( yaw r))) //yaw: sobre eje Z (tambi� llamado heading) //pitch: sobre eje Y //roll: sobre eje X float SinYaw = (float)sin(yaw/2); float SinPitch = (float)sin(pitch/2); float SinRoll = (float)sin(roll/2); float CosYaw = (float)cos(yaw/2); float CosPitch = (float)cos(pitch/2); float CosRoll = (float)cos(roll/2); x = SinRoll * CosPitch * CosYaw - CosRoll * SinPitch * SinYaw; y = CosRoll * SinPitch * CosYaw + SinRoll * CosPitch * SinYaw; z = CosRoll * CosPitch * SinYaw - SinRoll * SinPitch * CosYaw; w = CosRoll * CosPitch * CosYaw + SinRoll * SinPitch * SinYaw; } ///////////////////////////// //Operadores de asignaci� // ///////////////////////////// quat& quat::operator = ( const quat q ) { x = q.x; y = q.y; z = q.z; w = q.w; return *this; } inline quat& quat::operator += ( const quat q ) { x += q.x; y += q.y; z += q.z; w += q.w; return *this; } inline quat& quat::operator -= (const quat q) { x -= q.x; y -= q.y; z -= q.z; w -= q.w; return *this; } quat& quat::operator *= (const quat q) { //guardo los valores actuales float Angle1 = Angle; vect RotAx1(RotAx); //Los quaterniones se multiplican: //Angulo = ang1*ang2 - (RotAx1 o RotAx2) //Eje = ang1*V2 + ang2*V1 + (RotAx1 x RotAx2) //siendo 'o' prod escalar y 'x' prod vectorial //�gulo Angle = Angle1 * q.Angle - (VectDotProd(RotAx1, q.RotAx)); //vector RotAx = Angle1 * q.RotAx + q.Angle * RotAx1 + VectXProd(RotAx1, q.RotAx); return *this; } quat& quat::operator *= ( const float num) { x *= num; y *= num; z *= num; w *= num; return *this; } void quat::set(const float yaw, const float pitch, const float roll) { //idem al constructor a partir de �gulos Euler float SinYaw = (float)sin(yaw/2); float SinPitch = (float)sin(pitch/2); float SinRoll = (float)sin(roll/2); float CosYaw = (float)cos(yaw/2); float CosPitch = (float)cos(pitch/2); float CosRoll = (float)cos(roll/2); x = SinRoll * CosPitch * CosYaw - CosRoll * SinPitch * SinYaw; y = CosRoll * SinPitch * CosYaw + SinRoll * CosPitch * SinYaw; z = CosRoll * CosPitch * SinYaw - SinRoll * SinPitch * CosYaw; w = CosRoll * CosPitch * CosYaw + SinRoll * SinPitch * SinYaw; } void quat::set(const float x1, const float y1, const float z1, const float angle) { w = angle; x = x1; y = y1; z = z1; } //----------------------------------------------------------------------/ ////////////////////////////////////////////////////// //Implementaci� de los operadores de la clase mat4 // ////////////////////////////////////////////////////// ////////////////// //Constructores // ////////////////// mat4::mat4( float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) { m[0] = m00; m[1] = m01; m[2] = m02; m[3] = m03; m[4] = m10; m[5] = m11; m[6] = m12; m[7] = m13; m[8] = m20; m[9] = m21; m[10]= m22; m[11]= m23; m[12]= m30; m[13]= m31; m[14]= m32; m[15]= m33; } mat4::mat4(const mat4 &m) { for(int a = 0 ; a < 16; a++) this->m[a] = m.m[a]; } ///////////////////////////// //Operadores de asignaci� // ///////////////////////////// mat4& mat4::operator = ( const mat4& m ) { //resta miembro a miembro for(int a = 0 ; a < 16; a++) this->m[a] = m.m[a]; return *this; } mat4& mat4::operator += ( const mat4& m ) { //suma miembro a miembro for(int a = 0 ; a < 16; a++) this->m[a] += m.m[a]; return *this; } mat4& mat4::operator -= ( const mat4& m ) { for(int a = 0 ; a < 16; a++) this->m[a] -= m.m[a]; return *this; } mat4& mat4::operator *= ( const mat4& m ) { //la multiplicacion de una matriz por otra consiste en //hacer el producto escalar entre los vectores fila de //la primera por los vectores columna de la segunda que //donde se cruzan definen el valor a calcular //asi, si C = A * B //C[i][j] = A[i].B[j] //siendo A[i] -> vector fila i de la matriz A (4 componentes) // B[j] -> vector columna j de la matriz b (4 componentes) //el prod escalar era (x1,y1...).(x2,y2...) = x1*x2 + y1*y2 +.. mat4 c(*this); this->m1[0][0] = c.m1[0][0] * m.m1[0][0] + c.m1[1][0] * m.m1[0][1] + c.m1[2][0] * m.m1[0][2] + c.m1[3][0] * m.m1[0][3]; this->m1[0][1] = c.m1[0][1] * m.m1[0][0] + c.m1[1][1] * m.m1[0][1] + c.m1[2][1] * m.m1[0][2] + c.m1[3][1] * m.m1[0][3]; this->m1[0][2] = c.m1[0][2] * m.m1[0][0] + c.m1[1][2] * m.m1[0][1] + c.m1[2][2] * m.m1[0][2] + c.m1[3][2] * m.m1[0][3]; this->m1[0][3] = c.m1[0][3] * m.m1[0][0] + c.m1[1][3] * m.m1[0][1] + c.m1[2][3] * m.m1[0][2] + c.m1[3][3] * m.m1[0][3]; this->m1[1][0] = c.m1[0][0] * m.m1[1][0] + c.m1[1][0] * m.m1[1][1] + c.m1[2][0] * m.m1[1][2] + c.m1[3][0] * m.m1[1][3]; this->m1[1][1] = c.m1[0][1] * m.m1[1][0] + c.m1[1][1] * m.m1[1][1] + c.m1[2][1] * m.m1[1][2] + c.m1[3][1] * m.m1[1][3]; this->m1[1][2] = c.m1[0][2] * m.m1[1][0] + c.m1[1][2] * m.m1[1][1] + c.m1[2][2] * m.m1[1][2] + c.m1[3][2] * m.m1[1][3]; this->m1[1][3] = c.m1[0][3] * m.m1[1][0] + c.m1[1][3] * m.m1[1][1] + c.m1[2][3] * m.m1[1][2] + c.m1[3][3] * m.m1[1][3]; this->m1[2][0] = c.m1[0][0] * m.m1[2][0] + c.m1[1][0] * m.m1[2][1] + c.m1[2][0] * m.m1[2][2] + c.m1[3][0] * m.m1[2][3]; this->m1[2][1] = c.m1[0][1] * m.m1[2][0] + c.m1[1][1] * m.m1[2][1] + c.m1[2][1] * m.m1[2][2] + c.m1[3][1] * m.m1[2][3]; this->m1[2][2] = c.m1[0][2] * m.m1[2][0] + c.m1[1][2] * m.m1[2][1] + c.m1[2][2] * m.m1[2][2] + c.m1[3][2] * m.m1[2][3]; this->m1[2][3] = c.m1[0][3] * m.m1[2][0] + c.m1[1][3] * m.m1[2][1] + c.m1[2][3] * m.m1[2][2] + c.m1[3][3] * m.m1[2][3]; this->m1[3][0] = c.m1[0][0] * m.m1[3][0] + c.m1[1][0] * m.m1[3][1] + c.m1[2][0] * m.m1[3][2] + c.m1[3][0] * m.m1[3][3]; this->m1[3][1] = c.m1[0][1] * m.m1[3][0] + c.m1[1][1] * m.m1[3][1] + c.m1[2][1] * m.m1[3][2] + c.m1[3][1] * m.m1[3][3]; this->m1[3][2] = c.m1[0][2] * m.m1[3][0] + c.m1[1][2] * m.m1[3][1] + c.m1[2][2] * m.m1[3][2] + c.m1[3][2] * m.m1[3][3]; this->m1[3][3] = c.m1[0][3] * m.m1[3][0] + c.m1[1][3] * m.m1[3][1] + c.m1[2][3] * m.m1[3][2] + c.m1[3][3] * m.m1[3][3]; return *this; } mat4& mat4::operator *= ( const float d ) { for(int a = 0 ; a < 16; a++) this->m[a] *= d; return *this; } mat4& mat4::operator /= ( const float d ) { for(int a = 0 ; a < 16; a++) this->m[a] /= d; return *this; } //////////////////////////////////////////////////////////////// // / // Implementaciones de operadores friends para la clase mat4 / // / //////////////////////////////////////////////////////////////// ////////////////////// // Operador unario // ////////////////////// mat4 operator - (const mat4& a) { mat4 c; for (int i = 0; i < 16; i++) c.m[i] = -a.m[i]; return c; } ///////////////////////// // Operadores binarios // ///////////////////////// mat4 operator * (const mat4& a, const mat4& b) { mat4 c(a); c *= b; return c; } mat4 operator * (const mat4& a, const float d) { //multiplicacion miembro a miembro mat4 c; for (int i = 0; i < 16; i++) c.m[i] = a.m[i]*d; return c; } mat4 operator * (const float d, const mat4& a) { //multiplicacion miembro a miembro mat4 c; for (int i = 0; i < 16; i++) c.m[i] = a.m[i]*d; return c; } mat4 operator / (const mat4& a, const float d) { //es la division miembro a miembro mat4 b; float u = 1.0f / d; for (int i = 0; i < 16; i++) b.m[i] = a.m[i]*u; return b; } mat4 operator + (const mat4& a, const mat4& b) { //es la suma miembro a miembro mat4 c; for (int i = 0; i < 16; i++) c.m[i] = a.m[i]+b.m[i]; return c; } mat4 operator - (const mat4& a, const mat4& b) { //es la resta miembro a miembro mat4 c; for (int i = 0; i < 16; i++) c.m[i] = a.m[i]-b.m[i]; return c; } /////////////////////////////// // Operadores de comparaci� // /////////////////////////////// int operator == (const mat4& a, const mat4& b) { return ( (b.m[0] == a.m[0]) && (b.m[1] == a.m[1]) && (b.m[2] == a.m[2]) && (b.m[3] == a.m[3]) && (b.m[4] == a.m[4]) && (b.m[5] == a.m[5]) && (b.m[6] == a.m[6]) && (b.m[7] == a.m[7]) && (b.m[8] == a.m[8]) && (b.m[9] == a.m[9]) && (b.m[10] == a.m[10]) && (b.m[11] == a.m[11]) && (b.m[12] == a.m[12]) && (b.m[13] == a.m[13]) && (b.m[14] == a.m[14]) && (b.m[15] == a.m[15])); } int operator != (const mat4& a, const mat4& b) { return !(a==b); } ////////////////////////// // Funciones // ////////////////////////// void mat4::Transpose() { mat4 a(*this); /*--------------------------------------------------- La transpuesta es la que cambia filas por columnas o sea m[i][j] = m[j][i] (la diagonal no cambia) para una matriz de rotaci� u ortogonal, la trans- puesta es la inversa. ----------------------------------------------------*/ m1[0][1] = a.m1[1][0]; m1[0][2] = a.m1[2][0]; m1[0][3] = a.m1[3][0]; m1[1][2] = a.m1[2][1]; m1[1][3] = a.m1[3][1]; m1[2][3] = a.m1[3][2]; m1[1][0] = a.m1[0][1]; m1[2][0] = a.m1[0][2]; m1[3][0] = a.m1[0][3]; m1[2][1] = a.m1[1][2]; m1[3][1] = a.m1[1][3]; m1[3][2] = a.m1[2][3]; } /*------------------------------------------------------------- Saca la inversa de la matriz. NO ES LA TRANSPUESTA. siendo A' la inversa de A => A*A' = A'*A = I (mat identidad). La transformaci� de la inversa es exactamente contraria a la de la matriz. Si tenemos en cuenta que al multiplicar ma- trices se acumulan sus efectos es f�il ver porque la matriz por la inversa da la identidad (transformo y destransformo lo que da la identidad que no hace nada). En este caso se supone una parte rotacional ortogonal, por lo que la parte de rotaci� solo la trasponemos y adem� que el ltimo vector columna es (0,0,0,1). --------------------------------------------------------------*/ void mat4::Invert() { mat4 b = *this; b.m1[0][0] = m1[0][0]; b.m1[0][1] = m1[1][0]; b.m1[0][2] = m1[2][0]; b.m1[1][0] = m1[0][1]; b.m1[1][1] = m1[1][1]; b.m1[1][2] = m1[2][1]; b.m1[2][0] = m1[0][2]; b.m1[2][1] = m1[1][2]; b.m1[2][2] = m1[2][2]; /* el nuevo vector desplazamiento es: d' = -(R^-1) * d */ b.m1[3][0] = -( m1[3][0] * b.m1[0][0] + m1[3][1] * b.m1[1][0] + m1[3][2] * b.m1[2][0] ); b.m1[3][1] = -( m1[3][0] * b.m1[0][1] + m1[3][1] * b.m1[1][1] + m1[3][2] * b.m1[2][1] ); b.m1[3][2] = -( m1[3][0] * b.m1[0][2] + m1[3][1] * b.m1[1][2] + m1[3][2] * b.m1[2][2] ); /* el resto queda igual */ b.m1[0][3] = b.m1[1][3] = b.m1[2][3] = 0.0f; b.m1[3][3] = 1.0f; *this = b; } void mat4::MakeGL() { mat4 a(*this); //transpone la parte de rotaci� para hacerla compatible //con OpenGL que acomoda los vectores por columna. //Si tenemos la matriz en formato matem�ico (por filas) //es necesario antes de usarla para un glMultMatrix(). m1[0][1] = a.m1[1][0]; m1[0][2] = a.m1[2][0]; m1[1][2] = a.m1[2][1]; m1[1][0] = a.m1[0][1]; m1[2][0] = a.m1[0][2]; m1[2][1] = a.m1[1][2]; } //setea la matriz a una identidad void mat4::SetId(void) { m[0] = 1; m[1] = 0; m[2] = 0; m[3] = 0; m[4] = 0; m[5] = 1; m[6] = 0; m[7] = 0; m[8] = 0; m[9] = 0; m[10] = 1; m[11] = 0; m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1; } //crea la matriz de rotaci� a partir de un quat void mat4::FromQuat(const quat &q) { float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; // calculo coeficientes x2 = q.x + q.x; y2 = q.y + q.y; z2 = q.z + q.z; xx = q.x * x2; xy = q.x * y2; xz = q.x * z2; yy = q.y * y2; yz = q.y * z2; zz = q.z * z2; wx = q.w * x2; wy = q.w * y2; wz = q.w * z2; m[0] = 1.0f - (yy + zz); m[1] = xy - wz; m[2] = xz + wy; m[3] = 0.0f; m[4] = xy + wz; m[5] = 1.0f - (xx + zz); m[6] = yz - wx; m[7] = 0.0f; m[8] = xz - wy; m[9] = yz + wx; m[10] = 1.0f - (xx + yy); m[11] = 0.0f; m[12] = 0.0f; m[13] = 0.0f; m[14] = 0.0f; m[15] = 1.0f; } //Calcula la matriz de rotaci� a partir de una base (gralmente ortonormal) void mat4::FromAllVectors(const vect &vpn, const vect &vup, const vect &vr) { m1[0][0] = vr[0]; //->vr = View Rigth m1[1][0] = vr[1]; //(1,0,0) para la identidad m1[2][0] = vr[2]; m1[3][0] = 0; m1[0][1] = vup[0]; //->vpn = View Up m1[1][1] = vup[1]; //(0,1,0) para la identidad m1[2][1] = vup[2]; m1[3][1] = 0; m1[0][2] = vpn[0]; //->vpn = View Normal m1[1][2] = vpn[1]; //(0,0,1) para la identidad m1[2][2] = vpn[2]; m1[3][2] = 0; m1[0][3] = 0; m1[1][3] = 0; m1[2][3] = 0; m1[3][3] = 1; } //Setea una traslaci� dada void mat4::SetTranslation(const vect &pos) { m1[3][0] = pos[0]; m1[3][1] = pos[1]; m1[3][2] = pos[2]; } //devuelve la parte trasladora vect mat4::GetTranslation(void) { return vect(m1[3][0], m1[3][1], m1[3][2]); } //Suma una traslaci� dada void mat4::AddTranslation(const vect &pos) { m1[3][0] += pos[0]; m1[3][1] += pos[1]; m1[3][2] += pos[2]; } //devuelve el vector de la base vect mat4::GetViewUp(void) { return vect(m1[0][1], m1[1][1], m1[2][1]); } vect mat4::GetViewRight(void) { return vect(m1[0][0], m1[1][0], m1[2][0]); } vect mat4::GetViewNormal(void) { return vect(m1[0][2], m1[1][2], m1[2][2]); } //setea el vector de la base void mat4::SetViewUp(const vect &vup) { m1[0][1] = vup.x; m1[1][1] = vup.y; m1[2][1] = vup.z; } void mat4::SetViewRight(const vect &vr) { m1[0][0] = vr.x; m1[1][0] = vr.y; m1[2][0] = vr.z; } void mat4::SetViewNormal(const vect &vpn) { m1[0][2] = vpn.x; m1[1][2] = vpn.y; m1[2][2] = vpn.z; } //////////////////////////////////////////////////////////////// // / // Implementaci� de funciones matem�icas auxiliares / // / //////////////////////////////////////////////////////////////// /***************************************************************** * Function name : DegToRad * * Description : Conversi� Grados a Radianes en sus dos sabores* * Return type : void * * Argument : float * *****************************************************************/ float DegToRad(const float deg) { return deg*PI/180; } void DegToRad(float *deg) { *deg = (*deg)*PI/180; } /***************************************************************** * Function name : RadToDeg * * Description : Conversi� Radianes a Grados en sus dos sabores* * Return type : float * * Argument : float * *****************************************************************/ float RadToDeg(const float rad) { return rad*180/PI; } void RadToDeg(float *rad) { *rad = (*rad)*180/PI; } /**************************************************************** * Function name : ClampD-R * * Description : Keeps angles between 0 and 360/ 0/2PI * * Return type : void * * Argument : float * ****************************************************************/ void ClampD (float x) { while (x > 360.0f) x -= 360.0f; while (x < 0.0f) x += 360.0f; } void ClampR (float x) { while (x > PI2) x -= PI2; while (x < 0.0f) x += PI2; } /***************************************************************** * Function name : Sgn * * Description : Devuelve el signo de un float mediante acceso * * binario (mas r�ido que comparar con 0. * * Return type : int (-1 para negativo, 1 para positivo o cero) * * Argument : float * *****************************************************************/ int Sgn(float num) { //acceso al float como DWORD (32 bits idem al float) unsigned long fl = (*(unsigned long *)&(num)); fl = ((fl &= 0x80000000) >> 31 ) & 0x1; if (fl) return -1; else return 1; } /**************************************************************** * Function name : VectDotProd * * Description : Producto escalar de dos vectores. * * El orden de los vectores no afecta el * * resultado (conmutativo) * * Return type : float * * Argument : vect, vect * ****************************************************************/ float VectDotProd(const vect& vect1, const vect& vect2) { return (vect1.x * vect2.x + vect1.y * vect2.y + vect1.z * vect2.z); } /***************************************************************** * Function name : VectXProd * * Description : Producto vectorial de dos vectores. * * El orden de los vectores AFECTA el resultado. * * El resultado da un vector perpendicular al * * plano formado por los dos vectores argumento. * * De acuerdo al orden de los vectores pasados * * el resultado apunta para un lado o para el otro* * (o sea el orden afecta el sentido del vector * * resultado pero no su direccion ni m�ulo * * Return type : vect * * Argument : vect, vect * *****************************************************************/ vect VectXProd(const vect& vect1, const vect& vect2) { vect result ( vect1.y * vect2.z - vect2.y * vect1.z, vect2.x * vect1.z - vect1.x * vect2.z, vect1.x * vect2.y - vect2.x * vect1.y ); return result; //para entender la direccion del vector resultado se usa la regla //de la mano derecha. Pone tu indice apuntando al eje x positivo, //tu largo al z positivo y tu pulgar al y positivo. //Ahora si multiplicas X=(1,0,0) por Y=(0,1,0) el resultado es //Z=(0,0,1), perpendicular a X e Y y direccion eje Z positivo. //Pero si multiplicas Y*X (invertis los argumentos) el resultado //da Z = (0,0,-1), con sentido contrario al anterior pero misma //direccion (eje Z) y m�ulo. //Para saber el sentido que va a tener el resultado, con tu mano //derecha acomodada como te dije al ppio. pone el indice apuntando //al primer vector, el largo al segundo y el pulgar te va a marcar el //sentido del vector resultado. } /**************************************************************** * Function name : Mod * * Description : M�ulo de un vector o magnitud. * * Es el "largo" del vector y se saca por * * Pit�oras * * Return type : float * * Argument : vect * ****************************************************************/ float Mod(const vect& vect1) { return sqrt(vect1.x * vect1.x + vect1.y * vect1.y + vect1.z * vect1.z); } /**************************************************************** * Function name : ModSquared * * Description : M�ulo de un vector al cuadrado. * * Sirve para varias funciones * * Return type : float * * Argument : vect * ****************************************************************/ float ModSquared(const vect& vect1) { return vect1.x * vect1.x + vect1.y * vect1.y + vect1.z * vect1.z; } /**************************************************************** * Function name : Normalize * * Description : Normaliza un vector (hace su m�ulo = 1) * * Return type : void * * Argument : vect* * ****************************************************************/ void Normalize(vect *vect1) { float module = Mod(*vect1); if(module > EPS) { vect1->x /= module; vect1->y /= module; vect1->z /= module; } } /**************************************************************** * Function name : Normalize * * Description : Overload de Normalize. * * Devuelve un vector normalizado sin modificar * * argumento. * * Return type : vect. * * Argument : vect. * ****************************************************************/ vect Normalize(const vect& vect1) { vect result; float module = Mod(vect1); if (module > EPS) { result.x = vect1.x/module; result.y = vect1.y/module; result.z = vect1.z/module; } else { result.x = result.y = result.z = EPS; } return result; } /**************************************************************** * Function name : OrigToRect * * Description : Distancia del origen a una recta. * * Le pasas dos puntos que definan (pertenezcan) * * a la recta, devuelve la distancia minima ( a * * traves de la normal) * * Return type : float * * Argument : vect,vect * ****************************************************************/ float OrigToRect(const vect& point1, const vect& point2) { //me aseguro que los argumentos no sean iguales //(definen infinitas rectas) para no provocar una //divisi� por cero. if (point1 == point2) return 0; //Saco el vector directriz de la recta vect v = point2 - point1; //Saco el m�ulo cuadrado del directriz float modv2 = v.x * v.x + v.y * v.y + v.z * v.z; //Saco el prod escalar entre point1 y v float p1dotv = VectDotProd(point1,v); //Aprovecho la misma variable p1dotv/=modv2; //Saco el normal a v, con extremo en la recta point1-point2 vect n ( -p1dotv * v.x + point1.x, -p1dotv * v.y + point1.y, -p1dotv * v.z + point1.z ); //Devuelvo el m�ulo de ese vector return Mod(n); } /**************************************************************** * Function name : OrigToRect2 * * Description : Distancia del origen a una recta. * * Idem OrigToRect pero esta ves la recta se * * define por un vector direccion y un punto * * perteneciente. * * Return type : float * * Argument : vect,vect * ****************************************************************/ float OrigToRect2(const vect& dir, const vect& point) { //No hice un overload de la anterior porque tiene //los mismos argumentos y return pero los argumentos //tienen significados diferentes. //Me aseguro que dir no sea el vector nulo para evitar //divisi� por cero if (dir.x < EPS && dir.y < EPS && dir.z < EPS) return 0; //Saco el m�ulo cuadrado del directriz float modv2 = dir.x * dir.x + dir.y * dir.y + dir.z * dir.z; //Saco el prod escalar entre point y dir float p1dotv = VectDotProd(point,dir); //Aprovecho la misma variable p1dotv/=modv2; //Saco el normal a v, con extremo en la recta point1-point2 vect n ( -p1dotv * dir.x + point.x, -p1dotv * dir.y + point.y, -p1dotv * dir.z + point.z ); //Devuelvo el m�ulo de ese vector //Se puede aprovechar para devolver de paso n //que es la direccion return Mod(n); } /****************************************************************** * Function name : PointToRect * * Description : Distancia de un punto a una recta. * * Le pasas el punto y dos puntos mas que definan * * (pertenezcan) a la recta, devuelve la distancia * * entre el punto y la recta a traves de la normal * * O sea la distacia m�ima entre ambos. * * Return type : float * * Argument : vect,vect,vect * ******************************************************************/ float PointToRect (const vect& point, const vect& rect1, const vect& rect2) { //me aseguro que los puntos que definen la recta //no sea iguales (definan infinitas rectas) if (rect1 == rect2) return 0; //Saco el vector directriz de la recta //que seria rect2 - rect1 //Saco la direcci� del punto al primer punto que //define la recta que ser� point - rect1 //la distancia es el m�ulo del prod vectorial //sobre el m�ulo de la directriz return Mod(VectXProd(rect2 - rect1, point - rect1))/Mod(rect2 - rect1); } /****************************************************************** * Function name : OrigToPlane * * Description : Distancia del origen a un plano. * * El plano se define por 3 puntos (los argumentos)* * que pertenezcan al mismo * * Return type : float * * Argument : vect,vect,vect * ******************************************************************/ float OrigToPlane(const vect& point1, const vect& point2, const vect& point3) { //saco dos vectores pertenecientes al plano //y la normal al plano haciendo el prod. //vectorial de ellos vect Norm = VectXProd(point1 - point2, point3 - point2); //Haciendo el prod. escalar entre un punto de los tres //(cualquiera) y la normal obtengo la proyeccion de ese //vector sobre la normal, multip. por el m�ulo de la normal. float distance = VectDotProd(point1, Norm)/Mod(Norm); //la distancia puede dar + o - de acuerdo a la //orientaci� de la normal respecto del plano if (distance < 0) distance = -distance; return distance; } /****************************************************************** * Function name : PointToPlane * * Description : Distancia de un punto a un plano. * * El plano se define por 3 puntos (planeX) que * * pertenezcan al mismo. * * la distancia es a travez de la nornmal al plano * * o sea la minima distancia. * * Return type : float * * Argument : vect,vect,vect * ******************************************************************/ float PointToPlane(const vect& point, const vect& plane1, const vect& plane2, const vect& plane3) { //saco dos vectores pertenecientes al plano y con //ellos la normal al plano (haciendo el prod. vectorial) vect Norm = VectXProd(plane1 - plane2, plane3 - plane2); Normalize(&Norm); //Haciendo el prod. escalar entre un punto y la normal //obtengo la proyeccion del punto sobre la normal. float proj = VectDotProd(point, Norm); //Hago prod escalar de la normal por un punto del plano //para obtener la dist del origen al plano float distplan = VectDotProd(plane2, Norm); //la resta de la distancia al plano menos la proyeccion //del punto sobre la normal es la distancia buscada float distance = distplan - proj; //la distancia puede dar + o - de acuerdo a la //orientaci� de la normal respecto del plano if (distance < 0) distance = -distance; return distance; } /****************************************************************** * Function name : Distance * * Description : Distancia de un punto a otro. * * Return type : float * * Argument : vect,vect. * ******************************************************************/ float Distance (const vect& point1, const vect& point2) { return Mod(point2-point1); } /****************************************************************** * Function name : GetAngle * * Description : dados 3 puntos devuelve el �gulo formado * * por ellos con v�tice en point2. * * Devuelve en radianes, para converitr a grados * * usar RadToDeg() con el resultado. * * Return type : float * * Argument : vect,vect,vect * ******************************************************************/ float GetAngle (const vect& point1, const vect& point2, const vect& point3) { //Saco los vectores que van de 2->1 y de 2->3 vect v21 = Normalize(point1 - point2); vect v23 = Normalize(point3 - point2); //Hallo el prod. escalar de ambos. //El prod escalar es tambi�: //norma v21 * norma v23 * cos (angulo buscado) //como norma de v21 = norma v23 = 1 (porque los normalic� //el resultado es directamente el coseno del �gulo. float cos = VectDotProd(v21, v23); //Ahora tengo que hacer el coseno inverso (arco coseno) float angle = acos(cos); return angle; } /****************************************************************** * Function name : GetAngle * * Description : dados 2 vectores devuelve el �gulo formado * * por ellos. * * Devuelve en radianes, para grados usar RadToDeg * * Return type : float * * Argument : vect,vect. * ******************************************************************/ float GetAngle (const vect& vect1, const vect& vect2) { //identica a la anterior, pero esta vez ya //tengo los vectores float cos = VectDotProd(Normalize(vect1), Normalize(vect2)); float angle = acos(cos); return angle; } /****************************************************************** * Function name : MatfromQuat * * Description : dados un quaternion de rotaci� unitario com- * * pleta la matriz de rotaci� 4x4 equivalente. * * pasar un pointer a m[0][0] si es m[4][4] o * * m[0] si es m[16] * * Return type : void * * Argument : quat,float*. * ******************************************************************/ void MatrixfromQuat (const quat& q, float* m) { float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; // calculo coeficientes x2 = q.x + q.x; y2 = q.y + q.y; z2 = q.z + q.z; xx = q.x * x2; xy = q.x * y2; xz = q.x * z2; yy = q.y * y2; yz = q.y * z2; zz = q.z * z2; wx = q.w * x2; wy = q.w * y2; wz = q.w * z2; *(m) = 1.0 - (yy + zz); *(m+1) = xy - wz; *(m+2) = xz + wy; *(m+3) = 0.0; *(m+4) = xy + wz; *(m+5) = 1.0 - (xx + zz); *(m+6) = yz - wx; *(m+7) = 0.0; *(m+8) = xz - wy; *(m+9) = yz + wx; *(m+10) = 1.0 - (xx + yy); *(m+11) = 0.0; *(m+12) = 0; *(m+13) = 0; *(m+14) = 0; *(m+15) = 1; //Es asi por decreto =) } /****************************************************************** * Function name : QuatfromMat * * Description : dada una matriz de rotaci� de 4x4 completa el * * quaternion equivalente. * * La inversa de la anterior. * * Return type : void * * Argument : float* , quat. * ******************************************************************/ void QuatfromMatrix (const float *m, quat& q) { float qs2, qx2, qy2, qz2; // squared magniudes of quaternion components float tmp; int n; // primero calculamos los valores al cuadrado del quat. //por lo menos uno va a ser > 0 porque el quat es de m�ulo 1. qs2 = 0.25f * (*(m+0) + *(m+5) + *(m+10) + 1); qx2 = qs2 - 0.5f * (*(m+5) + *(m+10)); qy2 = qs2 - 0.5f * (*(m+10) + *(m+0)); qz2 = qs2 - 0.5f * (*(m+0) + *(m+5)); // encontramos el componente de magnitud m�ima n = (qs2 > qx2 ) ? ((qs2 > qy2) ? ((qs2 > qz2) ? 0 : 3) : ((qy2 > qz2) ? 2 : 3)) : ((qx2 > qy2) ? ((qx2 > qz2) ? 1 : 3) : ((qy2 > qz2) ? 2 : 3)); // calculamos los valores del quat switch(n) { case 0: q.w = sqrt(qs2); tmp = 0.25f / q.w; q.x = (*(m+9) - *(m+6)) * tmp; q.y = (*(m+2) - *(m+8)) * tmp; q.z = (*(m+4) - *(m+1)) * tmp; break; case 1: q.x = sqrt(qx2); tmp = 0.25f / q.x; q.w = (*(m+9) - *(m+6)) * tmp; q.y = (*(m+1) + *(m+4)) * tmp; q.z = (*(m+2) + *(m+8)) * tmp; break; case 2: q.y = sqrt(qy2); tmp = 0.25f / q.y; q.w = (*(m+2) - *(m+8)) * tmp; q.z = (*(m+6) + *(m+9)) * tmp; q.x = (*(m+4) + *(m+1)) * tmp; break; case 3: q.z = sqrt(qz2); tmp = 0.25f / q.z; q.w = (*(m+4) - *(m+1)) * tmp; q.x = (*(m+8) + *(m+2)) * tmp; q.y = (*(m+9) + *(m+6)) * tmp; break; } // forzamos escalar positivo. if (q.w < 0.0f) { q.w = -q.w; q.x = -q.x; q.y = -q.y; q.z = -q.z; } //otro decretazo... es asi. } /****************************************************************** * Function name : QuatfromEuler * * Description : dados �gulos de Euler completa el quaternion * * equivalente. El orden de operaci� es: * * p' = roll( pitch( yaw(p) ) ) * * * * Yaw: Respecto del Z * * Roll: Respecto del X * * Pitch: Respecto del Y * * * * Return type : void * * Argument : float, float, float, quat&. * ******************************************************************/ void QuatfromEuler( const float yaw, const float pitch, const float roll, quat& q) { float cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll; float half_roll, half_pitch, half_yaw; half_yaw = yaw / 2.0; half_pitch = pitch / 2.0; half_roll = roll / 2.0; cosYaw = cos(half_yaw); sinYaw = sin(half_yaw); cosPitch = cos(half_pitch); sinPitch = sin(half_pitch); cosRoll = cos(half_roll); sinRoll = sin(half_roll); q.x = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; q.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; q.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; } /***************************************************************** * Function name : QuatSlerp * * Description : Spherical Linear intERPolation entre dos quat. * * Un promedio ponderado de dos rotaciones. * * Return type : void * * Argument : &quat,&quat,float, &quat. * ******************************************************************/ void QuatSlerp(const quat &from, const quat &to, float t, quat &res) { //minlerp define la barrera para interpolar esf�ico/lineal //subir el valor para obtener mayor velocidad/menor calidad const float minlerp = 0.02f; quat to1; float omega, cosom, sinom, scale0, scale1; // calc cosine cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w; // adjust signs (if necessary) if ( cosom <0.0 ) { cosom = -cosom; to1.x = - to.x; to1.y = - to.y; to1.z = - to.z; to1.w = - to.w; } else { to1.x = to.x; to1.y = to.y; to1.z = to.z; to1.w = to.w; } // calculate coefficients if ( (1.0 - cosom) > minlerp ) { // interpolaci� esf�ica (SLERP) omega = (float)acos(cosom); sinom = (float)sin(omega); scale0 = (float)sin((1.0 - t) * omega) / sinom; scale1 = (float)sin(t * omega) / sinom; } else { // "from" y "to" quaternions est� muy cerca // uso interpolaci� lineal (LERP) scale0 = 1.0f - t; scale1 = t; } // calculate final values res.x = scale0 * from.x + scale1 * to1.x; res.y = scale0 * from.y + scale1 * to1.y; res.z = scale0 * from.z + scale1 * to1.z; res.w = scale0 * from.w + scale1 * to1.w; }