// =================================================== // MATH.CPP -=- Focus3 -=- By Jacco Bikker // Integer math 3D engine for mobile devices ><> 2003 // =================================================== #include "maths.h" #include "string.h" namespace Overloaded { void Vector::Between( Vector& a_V1, Vector& a_V2, int a_Distance ) { x = a_V1.x + ((a_V2.x - a_V1.x) >> 8) * (a_Distance >> 8); y = a_V1.y + ((a_V2.y - a_V1.y) >> 8) * (a_Distance >> 8); z = a_V1.z + ((a_V2.z - a_V1.z) >> 8) * (a_Distance >> 8); } int Vector::Distance( Plane& p, bool oldmethod ) { if (oldmethod) return Dot( p.GetNormal() ) - p.GetD(); else return Dot( p.GetNormal() ) + p.GetD(); } int Vector::Distance( Vector& v ) { Vector t = Vector( x, y, z ) - v; return t.Length(); } Vector Vector::ClosestPoint( Vector& a_C1, Vector& a_C2 ) { Vector v1 = *this - a_C1, v2 = a_C2 - a_C1; v2.Normalize(); int d = a_C1.Distance( a_C2 ); int t = v1.Dot( v2 ); if (t <= 0) return a_C1; else if (t >= d) return a_C2; else return a_C1 + v2 * t; } void Vector::Normalize() { double l = (double)Length() / 65536.0f; x = (int)((float)x / l); y = (int)((float)y / l); z = (int)((float)z / l); } int Vector::Length() { double fx = (double)x, fy = (double)y, fz = (double)z; return (int)sqrt( fx * fx + fy * fy + fz * fz ); } void Vector::Transform( Matrix& m ) { int ox = x, oy = y, oz = z; x = FPMUL( ox, m.Get( 0 ) ) + FPMUL( oy, m.Get( 1 ) ) + FPMUL( oz, m.Get( 2 ) ) + m.Get( 3 ); y = FPMUL( ox, m.Get( 4 ) ) + FPMUL( oy, m.Get( 5 ) ) + FPMUL( oz, m.Get( 6 ) ) + m.Get( 7 ); z = FPMUL( ox, m.Get( 8 ) ) + FPMUL( oy, m.Get( 9 ) ) + FPMUL( oz, m.Get( 10 ) ) + m.Get( 11 ); } void Vector::InvTransform( Matrix& m ) { int ox = x + m.Get( 3 ), oy = y + m.Get( 7 ), oz = z + m.Get( 11 ); x = FPMUL( ox, m.Get( 0 ) ) + FPMUL( oy, m.Get( 1 ) ) + FPMUL( oz, m.Get( 2 ) ); y = FPMUL( ox, m.Get( 4 ) ) + FPMUL( oy, m.Get( 5 ) ) + FPMUL( oz, m.Get( 6 ) ); z = FPMUL( ox, m.Get( 8 ) ) + FPMUL( oy, m.Get( 9 ) ) + FPMUL( oz, m.Get( 10 ) ); } void Matrix::Identity() { m_Cell[1] = m_Cell[2] = m_Cell[TX] = m_Cell[4] = m_Cell[6] = m_Cell[TY] = m_Cell[8] = m_Cell[9] = m_Cell[TZ] = m_Cell[12] = m_Cell[13] = m_Cell[14] =0; m_Cell[0] = m_Cell[5] = m_Cell[10] = m_Cell[15] = FPONE; m_Virgin = true; } void Matrix::LookAt( Vector& a_Eye, Vector& a_At, Vector& a_Up ) { Vector dir = a_At - a_Eye; dir.Normalize(); Vector r = dir.Cross( a_Up ); Vector u = r.Cross( dir ); m_Cell[0] = r.x; m_Cell[1] = r.y; m_Cell[2] = r.z; m_Cell[3] = 0; m_Cell[4] = u.x; m_Cell[5] = u.y; m_Cell[6] = u.z; m_Cell[7] = 0; m_Cell[8] = -dir.x; m_Cell[9] = -dir.y; m_Cell[10] = -dir.z; m_Cell[11] = 0; m_Virgin = false; } void Matrix::Concatenate( Matrix& m2 ) { Matrix res; if (m2.m_Virgin) { Vector V = m2.GetTranslation(); m_Cell[3] = m_Cell[3] + FPMUL( V.x, m_Cell[0] ) + FPMUL( V.y, m_Cell[1] ) + FPMUL( V.z, m_Cell[2] ); m_Cell[7] = m_Cell[7] + FPMUL( V.x, m_Cell[4] ) + FPMUL( V.y, m_Cell[5] ) + FPMUL( V.z, m_Cell[6] ); m_Cell[11] = m_Cell[11] + FPMUL( V.x, m_Cell[8] ) + FPMUL( V.y, m_Cell[9] ) + FPMUL( V.z, m_Cell[10] ); } else { for( int i = 0; i < 3; i++ ) { for( int j = 0; j < 3; j++ ) { res.m_Cell[i * 4 + j] = 0; res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 0] >> 1) * (m2.m_Cell[0 * 4 + j] >> 1)) >> 14; res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 1] >> 1) * (m2.m_Cell[1 * 4 + j] >> 1)) >> 14; res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 2] >> 1) * (m2.m_Cell[2 * 4 + j] >> 1)) >> 14; res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 3] >> 1) * (m2.m_Cell[3 * 4 + j] >> 1)) >> 14; } res.m_Cell[i * 4 + 3] = 0; res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 0] >> 4) * (m2.m_Cell[0 * 4 + 3] >> 4)) >> 8; res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 1] >> 4) * (m2.m_Cell[1 * 4 + 3] >> 4)) >> 8; res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 2] >> 4) * (m2.m_Cell[2 * 4 + 3] >> 4)) >> 8; res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 3] >> 6) * (m2.m_Cell[3 * 4 + 3] >> 6)) >> 4; } for( int j = 0; j < 4; j++ ) { res.m_Cell[12 + j] = 0; res.m_Cell[12 + j] += ((m_Cell[12] >> 4) * (m2.m_Cell[0 + j] >> 4)) >> 8; res.m_Cell[12 + j] += ((m_Cell[13] >> 4) * (m2.m_Cell[4 + j] >> 4)) >> 8; res.m_Cell[12 + j] += ((m_Cell[14] >> 4) * (m2.m_Cell[8 + j] >> 4)) >> 8; res.m_Cell[12 + j] += ((m_Cell[15] >> 4) * (m2.m_Cell[12 + j] >> 4)) >> 8; } memcpy( m_Cell, res.m_Cell, 4 * 16 ); } m_Virgin = false; } void Matrix::RotateX( int a_RX, int* a_Sin, int* a_Cos ) { int sx = a_Sin[a_RX]; int cx = a_Cos[a_RX]; Identity(); m_Cell[5] = cx; m_Cell[6] = sx; m_Cell[9] = -sx; m_Cell[10] = cx; m_Virgin = false; } void Matrix::RotateY( int a_RY, int* a_Sin, int* a_Cos ) { int sy = a_Sin[a_RY]; int cy = a_Cos[a_RY]; Identity(); m_Cell[0] = cy; m_Cell[2] = -sy; m_Cell[8] = sy; m_Cell[10] = cy; m_Virgin = false; } void Matrix::RotateZ( int a_RZ, int* a_Sin, int* a_Cos ) { int sz = a_Sin[a_RZ]; int cz = a_Cos[a_RZ]; Identity(); m_Cell[0] = cz; m_Cell[1] = sz; m_Cell[4] = -sz; m_Cell[5] = cz; m_Virgin = false; } void Matrix::Rotate( Vector a_Trans, int a_RX, int a_RY, int a_RZ, int* a_Sin, int* a_Cos ) { Matrix t; t.RotateX( a_RZ, a_Sin, a_Cos ); RotateY( a_RY, a_Sin, a_Cos ); Concatenate( t ); t.RotateZ( a_RX, a_Sin, a_Cos ); Concatenate( t ); Translate( a_Trans ); } void Matrix::Invert() { Matrix t; int tx = -m_Cell[3]; int ty = -m_Cell[7]; int tz = -m_Cell[11]; for ( int h = 0; h < 3; h++ ) for ( int v = 0; v < 3; v++ ) t.m_Cell[h + v * 4] = m_Cell[v + h * 4]; for ( int i = 0; i < 16; i++ ) m_Cell[i] = t.m_Cell[i]; m_Cell[3] = (((tx >> 4) * (m_Cell[0] >> 4)) >> 8) + (((ty >> 4) * (m_Cell[1] >> 4)) >> 8) + (((tz >> 4) * (m_Cell[2] >> 4)) >> 8); m_Cell[7] = (((tx >> 4) * (m_Cell[4] >> 4)) >> 8) + (((ty >> 4) * (m_Cell[5] >> 4)) >> 8) + (((tz >> 4) * (m_Cell[6] >> 4)) >> 8); m_Cell[11] = (((tx >> 4) * (m_Cell[8] >> 4)) >> 8) + (((ty >> 4) * (m_Cell[9] >> 4)) >> 8) + (((tz >> 4) * (m_Cell[10] >> 4)) >> 8); } Matrix Matrix::GetTransformMatrix() { Matrix retval = *this; retval.m_Cell[TX] = retval.m_Cell[TY] = retval.m_Cell[TZ] = 0; return retval; } Matrix Matrix::GetTranslationMatrix() { Matrix retval; retval.Translate( Vector( m_Cell[TX], m_Cell[TY], m_Cell[TZ] ) ); return retval; } Vector Matrix::GetTranslation() { return Vector( m_Cell[TX], m_Cell[TY], m_Cell[TZ] ); } void Matrix::CheckIdentity() { m_Virgin = ((m_Cell[0] != 1) && (m_Cell[1] != 0) && (m_Cell[2] != 0) && (m_Cell[4] != 0) && (m_Cell[5] != 1) && (m_Cell[6] != 0) && (m_Cell[8] != 0) && (m_Cell[9] != 0) && (m_Cell[10] != 1)); } void Plane::ABCD( Vector& v1, Vector& v2, Vector& v3) { Vector a = v2 - v1, b = v3 - v1; m_Normal.x = FPMUL( a.y, b.z ) - FPMUL( b.y, a.z ); m_Normal.y = FPMUL( a.z, b.x ) - FPMUL( b.z, a.x ); m_Normal.z = FPMUL( a.x, b.y ) - FPMUL( b.x, a.y ); m_Normal.Normalize(); m_D = m_Normal.Dot( v2 ); } Frustum::Frustum() { m_Planes = 0; for ( int i = 0; i < 10; i++ ) m_RotPlane[i] = new Plane(); } Frustum::~Frustum() { for (int i = 0; i < m_Planes; i++ ) delete m_Plane[i]; } void Frustum::Transform( Matrix& m ) { for ( int i = 0; i < m_Planes; i++ ) { Vector V, N = m_Plane[i]->GetNormal(); Vector T = m.GetTranslation(); V.x = FPMUL( N.x, m.Get( 0 ) ) + FPMUL( N.y, m.Get( 1 ) ) + FPMUL( N.z, m.Get( 2 ) ); V.y = FPMUL( N.x, m.Get( 4 ) ) + FPMUL( N.y, m.Get( 5 ) ) + FPMUL( N.z, m.Get( 6 ) ); V.z = FPMUL( N.x, m.Get( 8 ) ) + FPMUL( N.y, m.Get( 9 ) ) + FPMUL( N.z, m.Get( 10 ) ); m_RotPlane[i]->SetNormal( V ); m_RotPlane[i]->SetD( T.Dot( V ) ); } } void Frustum::Reset() { for (int i = 0; i < m_Planes; i++ ) delete m_Plane[i]; m_Planes = 0; } }; // namespace Overloaded // =================================================== // EOF