Serious-Engine/Sources/Engine/Math/Vector.h
2016-03-11 15:57:17 +02:00

562 lines
17 KiB
C++

/* Copyright (c) 2002-2012 Croteam Ltd. All rights reserved. */
#ifndef SE_INCL_VECTOR_H
#define SE_INCL_VECTOR_H
#ifdef PRAGMA_ONCE
#pragma once
#endif
#include <Engine/Base/Assert.h>
#include <Engine/Math/Matrix.h>
/*
* Template class for vector of arbitrary dimensions and arbitrary type of members
*/
template<class Type, int iDimensions>
class Vector {
public:
Type vector[iDimensions]; // array that holds the members
public:
/* Default constructor. */
__forceinline Vector(void);
/* Constructor from coordinates. */
__forceinline Vector(Type x1);
__forceinline Vector(Type x1, Type x2);
__forceinline Vector(Type x1, Type x2, Type x3);
__forceinline Vector(Type x1, Type x2, Type x3, Type x4);
/* Clear function */
__forceinline void Clear(void) {};
/* Conversion into scalar -- length of vector (Euclidian norm). */
__forceinline Type Length(void) const;
/* Conversion into scalar -- Manhattan norm of vector. */
__forceinline Type ManhattanNorm(void) const;
/* Conversion into scalar -- Max norm of vector. */
__forceinline Type MaxNorm(void) const;
/* Reference vector member by it's index (1-based indices!). */
__forceinline Type &operator()(int i);
__forceinline const Type &operator()(int i) const;
/* Normalize vector, i.e. make it a unit vector. */
__forceinline Vector<Type, iDimensions> &Normalize(void);
__forceinline Vector<Type, iDimensions> &SafeNormalize(void); // gives vector with (0,0,0) orientation if input is too small
/* Mathematical operators. */
// unary minus
__forceinline Vector<Type, iDimensions> &Flip(void);
__forceinline Vector<Type, iDimensions> operator-(void) const;
// between two vectors
__forceinline Vector<Type, iDimensions> operator+(const Vector<Type, iDimensions> &vector2) const;
__forceinline Vector<Type, iDimensions> &operator+=(const Vector<Type, iDimensions> &vector2);
__forceinline Vector<Type, iDimensions> operator-(const Vector<Type, iDimensions> &vector2) const;
__forceinline Vector<Type, iDimensions> &operator-=(const Vector<Type, iDimensions> &vector2);
// multiplication with scalar
__forceinline Vector<Type, iDimensions> &operator*=(const Type scalar);
__forceinline Vector<Type, iDimensions> operator*(const Type scalar) const;
// division with scalar
__forceinline Vector<Type, iDimensions> &operator/=(const Type scalar);
__forceinline Vector<Type, iDimensions> operator/(const Type scalar) const;
// multiplication by a square matrix (sides swapped -- see implementation for notes)
__forceinline Vector<Type, iDimensions> &operator*=(const Matrix<Type, iDimensions, iDimensions> &matrix2);
__forceinline Vector<Type, iDimensions> operator*(const Matrix<Type, iDimensions, iDimensions> &matrix2) const;
// scalar product - dot product, inner product
__forceinline Type operator%(const Vector<Type, iDimensions> &vector2) const;
// vector product - cross product, outer product
__forceinline Vector<Type, iDimensions> &operator*=(const Vector<Type, iDimensions> &vector2);
__forceinline Vector<Type, iDimensions> operator*(const Vector<Type, iDimensions> &vector2) const;
// comparing vectors
__forceinline BOOL operator==(const Vector<Type, iDimensions> &vector2) const;
__forceinline BOOL operator!=(const Vector<Type, iDimensions> &vector2) const;
/* Stream operations */
friend __forceinline CTStream &operator>>(CTStream &strm, Vector<Type, iDimensions> &vector) {
strm.Read_t(&vector, sizeof(vector));
return strm;
}
friend __forceinline CTStream &operator<<(CTStream &strm, Vector<Type, iDimensions> &vector) {
strm.Write_t(&vector, sizeof(vector));
return strm;
}
};
// inline functions implementation
/*
* Default constructor.
*/
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions>::Vector(void) {}
/*
* Constructor from coordinates.
*/
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions>::Vector(Type x1)
{
ASSERT(iDimensions==1);
(*this)(1)=x1;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions>::Vector(Type x1, Type x2)
{
ASSERT(iDimensions==2);
(*this)(1)=x1;
(*this)(2)=x2;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions>::Vector(Type x1, Type x2, Type x3)
{
ASSERT(iDimensions==3);
(*this)(1)=x1;
(*this)(2)=x2;
(*this)(3)=x3;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions>::Vector(Type x1, Type x2, Type x3, Type x4)
{
ASSERT(iDimensions==4);
(*this)(1)=x1;
(*this)(2)=x2;
(*this)(3)=x3;
(*this)(4)=x4;
}
/*
* Conversion into scalar -- length of vector.
*/
__forceinline FLOAT Vector<FLOAT,3>::Length(void) const
{
return (FLOAT)sqrt( (DOUBLE)((*this)(1)*(*this)(1) + (*this)(2)*(*this)(2) + (*this)(3)*(*this)(3)));
}
__forceinline DOUBLE Vector<DOUBLE,3>::Length(void) const
{
return (DOUBLE)sqrt( (DOUBLE)((*this)(1)*(*this)(1) + (*this)(2)*(*this)(2) + (*this)(3)*(*this)(3)));
}
template<class Type, int iDimensions>
__forceinline Type Vector<Type, iDimensions>::Length(void) const
{
Type result=(Type)0;
for(int i=1; i<=iDimensions; i++) {
result += (*this)(i) * (*this)(i);
}
return (Type)sqrt((DOUBLE)result);
}
/*
* Conversion into scalar -- Manhattan norm of vector.
*/
template<class Type, int iDimensions>
__forceinline Type Vector<Type, iDimensions>::ManhattanNorm(void) const
{
Type result=(Type)0;
for(int i=1; i<=iDimensions; i++) {
result += Abs((*this)(i));
}
return result;
}
/*
* Conversion into scalar -- Max norm of vector.
*/
template<class Type, int iDimensions>
__forceinline Type Vector<Type, iDimensions>::MaxNorm(void) const
{
Type result=(Type)0;
for(int i=1; i<=iDimensions; i++) {
result = Max(result, Abs((*this)(i)));
}
return result;
}
/*
* Reference vector member by it's index (1-based indices!).
*/
template<class Type, int iDimensions>
__forceinline Type &Vector<Type, iDimensions>::operator()(int i)
{
// check boundaries (indices start at 1, not at 0)
ASSERT(i>=1 && i<=iDimensions);
// return vector member reference
return vector[i-1];
}
template<class Type, int iDimensions>
__forceinline const Type &Vector<Type, iDimensions>::operator()(int i) const
{
// check boundaries (indices start at 1, not at 0)
ASSERT(i>=1 && i<=iDimensions);
// return vector member reference
return vector[i-1];
}
/*
* Normalize vector, i.e. make it a unit vector.
*/
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::Normalize(void)
{
// Normalizing a vector of a very small length can be very unprecise!
// ASSERT(((Type)*this) > 0.001);
*this/=Length();
return *this;
}
// gives vector with (0,0,0) orientation if input is too small
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::SafeNormalize(void)
{
Type tLen = Length();
if (tLen<1E-6) {
if (iDimensions==2) {
*this = Vector(1,0);
} else {
*this = Vector(0,0,-1);
}
} else {
*this/=tLen;
}
return *this;
}
// unary minus FLOAT3D
__forceinline Vector<FLOAT,3> &Vector<FLOAT,3>::Flip(void)
{
(*this)(1) = -(*this)(1);
(*this)(2) = -(*this)(2);
(*this)(3) = -(*this)(3);
return *this;
}
// unary minus DOUBLE3D
__forceinline Vector<DOUBLE,3> &Vector<DOUBLE,3>::Flip(void)
{
(*this)(1) = -(*this)(1);
(*this)(2) = -(*this)(2);
(*this)(3) = -(*this)(3);
return *this;
}
// unary minus
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::Flip(void)
{
// flip member by member
ASSERT( iDimensions!=3); // 3 is optimized special case
for(int iDimension=1; iDimension<=iDimensions; iDimension++) {
(*this)(iDimension) = -(*this)(iDimension);
}
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator-(void) const
{
return Vector<Type, iDimensions>(*this).Flip();
}
// sum of two vectors FLOAT3D
__forceinline Vector<FLOAT,3> &Vector<FLOAT,3>::operator+=(const Vector<FLOAT,3> &vector2)
{
// add member by member
(*this)(1) += vector2(1);
(*this)(2) += vector2(2);
(*this)(3) += vector2(3);
return *this;
}
// sum of two vectors DOUBLE3D
__forceinline Vector<DOUBLE,3> &Vector<DOUBLE,3>::operator+=(const Vector<DOUBLE,3> &vector2)
{
// add member by member
(*this)(1) += vector2(1);
(*this)(2) += vector2(2);
(*this)(3) += vector2(3);
return *this;
}
// sum of two vectors
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::operator+=(const Vector<Type, iDimensions> &vector2)
{
// add member by member
ASSERT( iDimensions!=3); // 3 is optimized special case
for(int iDimension=1; iDimension<=iDimensions; iDimension++) {
(*this)(iDimension) += vector2(iDimension);
}
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator+(const Vector<Type, iDimensions> &vector2) const
{
return Vector<Type, iDimensions>(*this)+=vector2;
}
// difference of two vectors FLOAT3D
__forceinline Vector<FLOAT,3> &Vector<FLOAT,3>::operator-=(const Vector<FLOAT,3> &vector2)
{
// add member by member
(*this)(1) -= vector2(1);
(*this)(2) -= vector2(2);
(*this)(3) -= vector2(3);
return *this;
}
// difference of two vectors DOUBLE3D
__forceinline Vector<DOUBLE,3> &Vector<DOUBLE,3>::operator-=(const Vector<DOUBLE,3> &vector2)
{
// add member by member
(*this)(1) -= vector2(1);
(*this)(2) -= vector2(2);
(*this)(3) -= vector2(3);
return *this;
}
// difference of two vectors
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::operator-=(const Vector<Type, iDimensions> &vector2)
{
// sub member by member
ASSERT( iDimensions!=3); // 3 is optimized special case
for(int iDimension=1; iDimension<=iDimensions; iDimension++) {
(*this)(iDimension) -= vector2(iDimension);
}
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator-(const Vector<Type, iDimensions> &vector2) const
{
return Vector<Type, iDimensions>(*this)-=vector2;
}
// multiplication with scalar FLOAT3D
__forceinline Vector<FLOAT,3> &Vector<FLOAT,3>::operator*=(const FLOAT scalar)
{
(*this)(1) *= scalar;
(*this)(2) *= scalar;
(*this)(3) *= scalar;
return *this;
}
// multiplication with scalar DOUBLE3D
__forceinline Vector<DOUBLE,3> &Vector<DOUBLE,3>::operator*=(const DOUBLE scalar)
{
(*this)(1) *= scalar;
(*this)(2) *= scalar;
(*this)(3) *= scalar;
return *this;
}
// multiplication with scalar
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::operator*=(const Type scalar)
{
ASSERT( iDimensions!=3); // 3 is optimized special case
for( int i=1; i<=iDimensions; i++) (*this)(i) *= scalar;
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator*(const Type scalar) const
{
return Vector<Type, iDimensions>(*this) *= scalar;
}
// division with scalar FLOAT3D
__forceinline Vector<FLOAT,3> &Vector<FLOAT,3>::operator/=(const FLOAT scalar)
{
const FLOAT rcp = 1.0f/scalar;
(*this)(1) *= rcp;
(*this)(2) *= rcp;
(*this)(3) *= rcp;
return *this;
}
// division with scalar DOUBLE3D
__forceinline Vector<DOUBLE,3> &Vector<DOUBLE,3>::operator/=(const DOUBLE scalar)
{
const DOUBLE rcp = 1.0/scalar;
(*this)(1) *= rcp;
(*this)(2) *= rcp;
(*this)(3) *= rcp;
return *this;
}
// division with scalar
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::operator/=(const Type scalar)
{
ASSERT( iDimensions!=3); // 3 is optimized special case
for( int i=1; i<=iDimensions; i++) (*this)(i) /= scalar;
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator/(const Type scalar) const
{
return Vector<Type, iDimensions>(*this) /= scalar;
}
/*
* Multiplication of a vector by a square matrix.
*/
// NOTE: The matrix should have been on the left side of the vector, but the template syntax wouldn't allow that.
__forceinline Vector<DOUBLE,3> Vector<DOUBLE,3>::operator*(const Matrix<DOUBLE,3,3> &matrix2) const
{
Vector<DOUBLE,3> result;
result(1) = matrix2(1,1) * (*this)(1) + matrix2(1,2) * (*this)(2) + matrix2(1,3) * (*this)(3);
result(2) = matrix2(2,1) * (*this)(1) + matrix2(2,2) * (*this)(2) + matrix2(2,3) * (*this)(3);
result(3) = matrix2(3,1) * (*this)(1) + matrix2(3,2) * (*this)(2) + matrix2(3,3) * (*this)(3);
return result;
}
__forceinline Vector<FLOAT,3> Vector<FLOAT,3>::operator*(const Matrix<FLOAT,3,3> &matrix2) const
{
Vector<FLOAT,3> result;
result(1) = matrix2(1,1) * (*this)(1) + matrix2(1,2) * (*this)(2) + matrix2(1,3) * (*this)(3);
result(2) = matrix2(2,1) * (*this)(1) + matrix2(2,2) * (*this)(2) + matrix2(2,3) * (*this)(3);
result(3) = matrix2(3,1) * (*this)(1) + matrix2(3,2) * (*this)(2) + matrix2(3,3) * (*this)(3);
return result;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::operator*=(const Matrix<Type, iDimensions, iDimensions> &matrix2)
{
(*this) = (*this) * matrix2;
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator*(const Matrix<Type, iDimensions, iDimensions> &matrix2) const
{
ASSERT( iDimensions!=3); // 3 is optimized special case
Vector<Type, iDimensions> result;
for(int iRow=1; iRow<=iDimensions; iRow++) {
result(iRow) = (Type)0;
for(int s=1; s<=iDimensions; s++) {
result(iRow) += matrix2(iRow, s) * (*this)(s);
}
}
return result;
}
// scalar product - dot product, inner product for FLOAT3D
__forceinline FLOAT Vector<FLOAT,3>::operator%(const Vector<FLOAT,3> &vector2) const
{
return (FLOAT)((*this)(1)*vector2(1) + (*this)(2)*vector2(2) + (*this)(3)*vector2(3));
}
// scalar product - dot product, inner product for DOUBLE3D
__forceinline DOUBLE Vector<DOUBLE,3>::operator%(const Vector<DOUBLE,3> &vector2) const
{
return (DOUBLE)((*this)(1)*vector2(1) + (*this)(2)*vector2(2) + (*this)(3)*vector2(3));
}
// scalar product - dot product, inner product
template<class Type, int iDimensions>
__forceinline Type Vector<Type, iDimensions>::operator%(const Vector<Type, iDimensions> &vector2) const
{
ASSERT( iDimensions!=3); // 3 is optimized special case
Type result=(Type)0;
for(int i=1; i<=iDimensions; i++) {
result += (*this)(i) * vector2(i);
}
return result;
}
// vector product - cross product, outer product
/* Formula: C=A*B
Cx = Ay*Bz - Az*By
Cy = Az*Bx - Ax*Bz
Cz = Ax*By - Ay*Bx
*/
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> &Vector<Type, iDimensions>::operator*=(const Vector<Type, iDimensions> &vector2)
{
(*this) = (*this) * vector2;
return *this;
}
template<class Type, int iDimensions>
__forceinline Vector<Type, iDimensions> Vector<Type, iDimensions>::operator*(const Vector<Type, iDimensions> &vector2) const
{
Vector<Type, iDimensions> result;
ASSERT(iDimensions==3); // cross product is defined only for 3D vectors
result(1) = (*this)(2)*vector2(3) - (*this)(3)*vector2(2);
result(2) = (*this)(3)*vector2(1) - (*this)(1)*vector2(3);
result(3) = (*this)(1)*vector2(2) - (*this)(2)*vector2(1);
return result;
}
// comparation FLOAT3D
__forceinline BOOL Vector<FLOAT,3>::operator==(const Vector<FLOAT,3> &vector2) const
{
return( (*this)(1)==vector2(1) && (*this)(2)==vector2(2) && (*this)(3)==vector2(3));
}
// comparation DOUBLE3D
__forceinline BOOL Vector<DOUBLE,3>::operator==(const Vector<DOUBLE,3> &vector2) const
{
return( (*this)(1)==vector2(1) && (*this)(2)==vector2(2) && (*this)(3)==vector2(3));
}
// comparation
template<class Type, int iDimensions>
__forceinline BOOL Vector<Type, iDimensions>::operator==(const Vector<Type, iDimensions> &vector2) const
{
ASSERT( iDimensions!=3); // 3 is optimized special case
for(int i=1; i<=iDimensions; i++) {
if( (*this)(i) != vector2(i))
return FALSE;
}
return TRUE;
}
template<class Type, int iDimensions>
__forceinline BOOL Vector<Type, iDimensions>::operator!=(const Vector<Type, iDimensions> &vector2) const
{
return !(*this == vector2);
}
// helper functions for converting between FLOAT and DOUBLE vectors
__forceinline DOUBLE3D FLOATtoDOUBLE(const FLOAT3D &vf)
{
return DOUBLE3D(FLOATtoDOUBLE(vf(1)), FLOATtoDOUBLE(vf(2)), FLOATtoDOUBLE(vf(3)));
}
__forceinline FLOAT3D DOUBLEtoFLOAT(const DOUBLE3D &vd)
{
return FLOAT3D(DOUBLEtoFLOAT(vd(1)), DOUBLEtoFLOAT(vd(2)), DOUBLEtoFLOAT(vd(3)));
}
#endif /* include-once check. */