269 lines
9.4 KiB
C++
269 lines
9.4 KiB
C++
/////////////////////////////////////////////////////////////////////////////
|
|
// Name: mathstuff.cpp
|
|
// Purpose: Some maths used for pyramid sample
|
|
// Author: Manuel Martin
|
|
// Created: 2015/01/31
|
|
// Copyright: (c) 2015 Manuel Martin
|
|
// Licence: wxWindows licence
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
|
|
#include <cmath>
|
|
|
|
#include "mathstuff.h"
|
|
|
|
// Overload of "-" operator
|
|
myVec3 operator- (const myVec3& v1, const myVec3& v2)
|
|
{
|
|
return myVec3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
|
|
}
|
|
|
|
// Vector normalization
|
|
myVec3 MyNormalize(const myVec3& v)
|
|
{
|
|
double mo = sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
|
|
if ( mo > 1E-20 )
|
|
return myVec3(v.x / mo, v.y / mo, v.z / mo);
|
|
else
|
|
return myVec3();
|
|
}
|
|
|
|
// Dot product
|
|
double MyDot(const myVec3& v1, const myVec3& v2)
|
|
{
|
|
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z ;
|
|
}
|
|
|
|
// Cross product
|
|
myVec3 MyCross(const myVec3& v1, const myVec3& v2)
|
|
{
|
|
return myVec3( v1.y * v2.z - v2.y * v1.z,
|
|
v1.z * v2.x - v2.z * v1.x,
|
|
v1.x * v2.y - v2.x * v1.y );
|
|
}
|
|
|
|
// Distance between two points
|
|
double MyDistance(const myVec3& v1, const myVec3& v2)
|
|
{
|
|
double rx = v1.x -v2.x;
|
|
double ry = v1.y -v2.y;
|
|
double rz = v1.z -v2.z;
|
|
|
|
return sqrt(rx*rx + ry*ry + rz*rz);
|
|
}
|
|
|
|
// Angle between two normalized vectors, in radians
|
|
double AngleBetween(myVec3 v1, myVec3 v2)
|
|
{
|
|
double angle = MyDot(v1, v2);
|
|
// Prevent issues due to numerical precision
|
|
if (angle > 1.0)
|
|
angle = 1.0;
|
|
if (angle < -1.0)
|
|
angle = -1.0;
|
|
|
|
return acos(angle);
|
|
}
|
|
|
|
// Matrix 4x4 by 4x1 multiplication
|
|
// Attention: No bounds check!
|
|
myVec4 MyMatMul4x1(const double *m1, const myVec4& v)
|
|
{
|
|
myVec4 mmv;
|
|
mmv.x = m1[0] * v.x + m1[4] * v.y + m1[8] * v.z + m1[12] * v.w ;
|
|
mmv.y = m1[1] * v.x + m1[5] * v.y + m1[9] * v.z + m1[13] * v.w ;
|
|
mmv.z = m1[2] * v.x + m1[6] * v.y + m1[10] * v.z + m1[14] * v.w ;
|
|
mmv.w = m1[3] * v.x + m1[7] * v.y + m1[11] * v.z + m1[15] * v.w ;
|
|
|
|
return mmv;
|
|
}
|
|
|
|
// Matrix 4x4 multiplication
|
|
// Attention: No bounds check!
|
|
void MyMatMul4x4(const double *m1, const double *m2, double* mm)
|
|
{
|
|
mm[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3] ;
|
|
mm[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3] ;
|
|
mm[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3] ;
|
|
mm[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3] ;
|
|
mm[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7] ;
|
|
mm[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7] ;
|
|
mm[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7] ;
|
|
mm[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7] ;
|
|
mm[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11] ;
|
|
mm[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11] ;
|
|
mm[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11] ;
|
|
mm[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11] ;
|
|
mm[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15] ;
|
|
mm[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15] ;
|
|
mm[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15] ;
|
|
mm[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15] ;
|
|
}
|
|
|
|
// Matrix 4x4 inverse. Returns the determinant.
|
|
// Attention: No bounds check!
|
|
// Method used is "adjugate matrix" with "cofactors".
|
|
// A faster method, such as "LU decomposition", isn't much faster than this code.
|
|
double MyMatInverse(const double *m, double *minv)
|
|
{
|
|
double det;
|
|
double cof[16], sdt[19];
|
|
|
|
// The 2x2 determinants used for cofactors
|
|
sdt[0] = m[10] * m[15] - m[14] * m[11] ;
|
|
sdt[1] = m[9] * m[15] - m[13] * m[11] ;
|
|
sdt[2] = m[9] * m[14] - m[13] * m[10] ;
|
|
sdt[3] = m[8] * m[15] - m[12] * m[11] ;
|
|
sdt[4] = m[8] * m[14] - m[12] * m[10] ;
|
|
sdt[5] = m[8] * m[13] - m[12] * m[9] ;
|
|
sdt[6] = m[6] * m[15] - m[14] * m[7] ;
|
|
sdt[7] = m[5] * m[15] - m[13] * m[7] ;
|
|
sdt[8] = m[5] * m[14] - m[13] * m[6] ;
|
|
sdt[9] = m[4] * m[15] - m[12] * m[7] ;
|
|
sdt[10] = m[4] * m[14] - m[12] * m[6] ;
|
|
sdt[11] = m[5] * m[15] - m[13] * m[7] ;
|
|
sdt[12] = m[4] * m[13] - m[12] * m[5] ;
|
|
sdt[13] = m[6] * m[11] - m[10] * m[7] ;
|
|
sdt[14] = m[5] * m[11] - m[9] * m[7] ;
|
|
sdt[15] = m[5] * m[10] - m[9] * m[6] ;
|
|
sdt[16] = m[4] * m[11] - m[8] * m[7] ;
|
|
sdt[17] = m[4] * m[10] - m[8] * m[6] ;
|
|
sdt[18] = m[4] * m[9] - m[8] * m[5] ;
|
|
// The cofactors, transposed
|
|
cof[0] = m[5] * sdt[0] - m[6] * sdt[1] + m[7] * sdt[2] ;
|
|
cof[1] = - m[1] * sdt[0] + m[2] * sdt[1] - m[3] * sdt[2] ;
|
|
cof[2] = m[1] * sdt[6] - m[2] * sdt[7] + m[3] * sdt[8] ;
|
|
cof[3] = - m[1] * sdt[13] + m[2] * sdt[14] - m[3] * sdt[15] ;
|
|
cof[4] = - m[4] * sdt[0] + m[6] * sdt[3] - m[7] * sdt[4] ;
|
|
cof[5] = m[0] * sdt[0] - m[2] * sdt[3] + m[3] * sdt[4] ;
|
|
cof[6] = - m[0] * sdt[6] + m[2] * sdt[9] - m[3] * sdt[10] ;
|
|
cof[7] = m[0] * sdt[13] - m[2] * sdt[16] + m[3] * sdt[17] ;
|
|
cof[8] = m[4] * sdt[1] - m[5] * sdt[3] + m[7] * sdt[5] ;
|
|
cof[9] = - m[0] * sdt[1] + m[1] * sdt[3] - m[3] * sdt[5] ;
|
|
cof[10] = m[0] * sdt[11] - m[1] * sdt[9] + m[3] * sdt[12] ;
|
|
cof[11] = - m[0] * sdt[14] + m[1] * sdt[16] - m[3] * sdt[18] ;
|
|
cof[12] = - m[4] * sdt[2] + m[5] * sdt[4] - m[6] * sdt[5] ;
|
|
cof[13] = m[0] * sdt[2] - m[1] * sdt[4] + m[2] * sdt[5] ;
|
|
cof[14] = - m[0] * sdt[8] + m[1] * sdt[10] - m[2] * sdt[12] ;
|
|
cof[15] = m[0] * sdt[15] - m[1] * sdt[17] + m[2] * sdt[18] ;
|
|
|
|
det = m[0] * cof[0] + m[1] * cof[4] + m[2] * cof[8] + m[3] * cof[12] ;
|
|
|
|
if ( fabs(det) > 10E-9 ) // Some precision value
|
|
{
|
|
double invdet = 1.0 / det;
|
|
for (int i = 0; i < 16; ++i)
|
|
minv[i] = cof[i] * invdet;
|
|
}
|
|
else
|
|
{
|
|
// Enable comparison with 0
|
|
det = 0.0;
|
|
}
|
|
|
|
return det;
|
|
}
|
|
|
|
// Matrix of rotation around an axis in the origin.
|
|
// angle is positive if follows axis (right-hand rule)
|
|
// Attention: No bounds check!
|
|
void MyRotate(const myVec3& axis, double angle, double *mrot)
|
|
{
|
|
double c = cos(angle);
|
|
double s = sin(angle);
|
|
double t = 1.0 - c;
|
|
|
|
// Normalize the axis vector
|
|
myVec3 uv = MyNormalize(axis);
|
|
|
|
// Store the matrix in column order
|
|
mrot[0] = t * uv.x * uv.x + c ;
|
|
mrot[1] = t * uv.x * uv.y + s * uv.z ;
|
|
mrot[2] = t * uv.x * uv.z - s * uv.y ;
|
|
mrot[3] = 0.0 ;
|
|
mrot[4] = t * uv.y * uv.x - s * uv.z ;
|
|
mrot[5] = t * uv.y * uv.y + c ;
|
|
mrot[6] = t * uv.y * uv.z + s * uv.x ;
|
|
mrot[7] = 0.0 ;
|
|
mrot[8] = t * uv.z * uv.x + s * uv.y ;
|
|
mrot[9] = t * uv.z * uv.y - s * uv.x ;
|
|
mrot[10] = t * uv.z * uv.z + c ;
|
|
mrot[11] = 0.0 ;
|
|
mrot[12] = mrot[13] = mrot[14] = 0.0 ;
|
|
mrot[15] = 1.0 ;
|
|
}
|
|
|
|
// Matrix for defining the viewing transformation
|
|
// Attention: No bounds check!
|
|
// Unchecked conditions:
|
|
// camPos != targ && camUp != {0,0,0}
|
|
// camUo can't be parallel to camPos - targ
|
|
void MyLookAt(const myVec3& camPos, const myVec3& camUp, const myVec3& targ, double *mt)
|
|
{
|
|
myVec3 tc = MyNormalize(targ - camPos);
|
|
myVec3 up = MyNormalize(camUp);
|
|
// Normalize tc x up for the case where up is not perpendicular to tc
|
|
myVec3 s = MyNormalize(MyCross(tc, up));
|
|
myVec3 u = MyNormalize(MyCross(s, tc)); //Normalize to improve accuracy
|
|
|
|
// Store the matrix in column order
|
|
mt[0] = s.x ;
|
|
mt[1] = u.x ;
|
|
mt[2] = - tc.x ;
|
|
mt[3] = 0.0 ;
|
|
mt[4] = s.y ;
|
|
mt[5] = u.y ;
|
|
mt[6] = - tc.y ;
|
|
mt[7] = 0.0 ;
|
|
mt[8] = s.z ;
|
|
mt[9] = u.z ;
|
|
mt[10] = - tc.z ;
|
|
mt[11] = 0.0 ;
|
|
mt[12] = - MyDot(s, camPos) ;
|
|
mt[13] = - MyDot(u, camPos) ;
|
|
mt[14] = MyDot(tc, camPos) ;
|
|
mt[15] = 1.0 ;
|
|
}
|
|
|
|
// Matrix for defining the perspective projection with symmetric frustum
|
|
// From camera coordinates to canonical (2x2x2 cube) coordinates.
|
|
// Attention: No bounds check!
|
|
// Unchecked conditions: fov > 0 && zNear > 0 && zFar > zNear && aspect > 0
|
|
void MyPerspective(double fov, double aspect, double zNear, double zFar, double *mp)
|
|
{
|
|
double f = 1.0 / tan(fov / 2.0);
|
|
|
|
// Store the matrix in column order
|
|
mp[0] = f / aspect ;
|
|
mp[1] = mp[2] = mp[3] = 0.0 ;
|
|
mp[4] = 0.0 ;
|
|
mp[5] = f ;
|
|
mp[6] = mp[7] = 0.0 ;
|
|
mp[8] = mp[9] = 0.0 ;
|
|
mp[10] = (zNear + zFar) / (zNear - zFar) ;
|
|
mp[11] = -1.0 ;
|
|
mp[12] = mp[13] = 0.0 ;
|
|
mp[14] = 2.0 * zNear * zFar / (zNear - zFar) ;
|
|
mp[15] = 0.0 ;
|
|
}
|
|
|
|
// Matrix for defining the orthogonal projection with symmetric frustum
|
|
// From camera coordinates to canonical (2x2x2 cube) coordinates.
|
|
// Attention: No bounds check!
|
|
// Unchecked conditions: left != right && bottom != top && zNear != zFar
|
|
void MyOrtho(double left, double right, double bottom, double top,
|
|
double zNear, double zFar, double *mo)
|
|
{
|
|
// Store the matrix in column order
|
|
mo[0] = 2.0 / (right - left) ;
|
|
mo[1] = mo[2] = mo[3] = mo[4] = 0.0 ;
|
|
mo[5] = 2.0 / (top - bottom) ;
|
|
mo[6] = mo[7] = mo[8] = mo[9] = 0.0 ;
|
|
mo[10] = 2.0 / (zNear - zFar) ;
|
|
mo[11] = 0.0 ;
|
|
mo[12] = -(right + left) / (right - left) ;
|
|
mo[13] = -(top + bottom) / (top - bottom) ;
|
|
mo[14] = (zNear + zFar) / (zNear - zFar) ;
|
|
mo[15] = 1.0 ;
|
|
}
|
|
|