glfxvec.h

/*-------------------------------------------------------------------------
This source code is part of GLfx library (OpenGL effect library)
For more information and updates see http://glfx.sourceforge.net/
Copyright (C) 2005-6 Tomek Rozen <trozen at users sourceforge net>

This library is free software; you can redistribute it and/or modify it
under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License,
or (at your option) any later version.

This library is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
for more details.
-------------------------------------------------------------------------*/

// Version: 8 (increment this number with every significant change)
// Note that this file may be used without the rest of GLFX library
// as it does not depend on other headers and source files
// (all declarations and implementation is inlined into this file).

#ifndef __GLFXVEC_H__
#define __GLFXVEC_H__

#include <math.h>
#include <assert.h>

#pragma pack(4)

namespace GLFX {

// ========================================================================
// Types and constants

const float PI = 3.14159265358979323846f;
const float TWOPI = PI * 2;
const float EPSILON = 1e-4f;

// ========================================================================
// Helper functions

inline float ToRadians(float degrees) {
    return degrees * PI / 180.0f;
}

inline float ToDegrees(float radians) {
    return radians * 180.0f / PI;
}

// ========================================================================
// Types

struct Vec2;
struct Vec3;
struct Vec4;
struct Quat;
struct Matrix3;
struct Matrix4;
struct Color;

// ========================================================================
// Vectors

struct Vec2
{
    float x;
    float y;

    inline Vec2();
    inline Vec2(const float* v);
    inline Vec2(const Vec2& v);
    inline Vec2(float x, float y);
    inline Vec2(float f);

    inline operator float*();
    inline operator const float*() const;
    inline operator Vec3() const;
    inline operator Vec4() const;
    inline float& operator[](int n);
    inline Vec2& operator=(const Vec2& v);
    inline Vec2& operator*=(const Vec2& v);
    inline Vec2& operator/=(const Vec2& v);
    inline Vec2& operator+=(const Vec2& v);
    inline Vec2& operator-=(const Vec2& v);

    inline Vec2& Normalize();
    inline float Length() const;
    inline bool Equal(const Vec2& v) const;
    inline bool NotEqual(const Vec2& v)const ;
    
    friend Vec2 operator*(const Vec2& a, const Vec2& b);
    friend Vec2 operator/(const Vec2& a, const Vec2& b);
    friend Vec2 operator+(const Vec2& a, const Vec2& b);
    friend Vec2 operator+(const Vec2& v);
    friend Vec2 operator-(const Vec2& a, const Vec2& b);
    friend Vec2 operator-(const Vec2& v);
    friend bool operator==(const Vec2& a, const Vec2& b);
    friend bool operator!=(const Vec2& a, const Vec2& b);

    friend Vec2 Perpendicular(const Vec2& v);
    friend float Dot(const Vec2& a, const Vec2& b);
    friend Vec2 Normalize(const Vec2& v);
    friend Vec2 Lerp(const Vec2& a, const Vec2& b, float f);
    friend float Length(const Vec2& a);
};

struct Vec3
{

    float x;
    float y;
    float z;

    inline Vec3();
    inline Vec3(const float* v);
    inline Vec3(const Vec3& v);
    inline Vec3(float x, float y, float z);
    inline Vec3(float f);

    inline operator float*();
    inline operator const float*() const;
    inline operator Vec2() const;
    inline operator Vec4() const;
    inline float& operator[](int n);
    inline Vec3& operator=(const Vec3& v);
    inline Vec3& Normalize();
    inline float Length() const;
    inline bool Equal(const Vec3& v) const;
    inline bool NotEqual(const Vec3& v) const;
    inline Vec3 MakePlanar(const Vec3& v) const;

    inline Vec3& operator*=(const Vec3& v);
    inline Vec3& operator/=(const Vec3& v);
    inline Vec3& operator+=(const Vec3& v);
    inline Vec3& operator-=(const Vec3& v);
    
    friend Vec3 operator*(const Vec3& a, const Vec3& b);
    friend Vec3 operator/(const Vec3& a, const Vec3& b);
    friend Vec3 operator+(const Vec3& a, const Vec3& b);
    friend Vec3 operator+(const Vec3& v);
    friend Vec3 operator-(const Vec3& a, const Vec3& b);
    friend Vec3 operator-(const Vec3& v);
    friend bool operator==(const Vec3& a, const Vec3& b);
    friend bool operator!=(const Vec3& a, const Vec3& b);
    friend bool operator<(const Vec3& a, const Vec3& b);

    friend Vec3 Cross(const Vec3& a, const Vec3& b);
    friend float Dot(const Vec3& a, const Vec3& b);
    friend Vec3 Normalize(const Vec3& v);
    friend Vec3 Lerp(const Vec3& a, const Vec3& b, float f);
    friend Vec3 RotateVectorX(const Vec3& v, float angle);
    friend Vec3 RotateVectorY(const Vec3& v, float angle);
    friend Vec3 RotateVectorZ(const Vec3& v, float angle);
    friend float Length(const Vec3& a);

};

struct Vec4
{

    float x;
    float y;
    float z;
    float w;

    inline Vec4();
    inline Vec4(const float* v);
    inline Vec4(const Vec4& v);
    inline Vec4(const Vec3& v, float w);
    inline Vec4(float x, float y, float z, float w);
    inline Vec4(float f);

    inline operator float*();
    inline operator const float*() const;
    inline operator Vec3() const;
    inline operator const Vec3&() const;
    inline float& operator[](int n);
    inline Vec4& operator=(const Vec4& v);
    inline Vec4& operator*=(const Vec4& v);
    inline Vec4& operator/=(const Vec4& v);
    inline Vec4& operator+=(const Vec4& v);
    inline Vec4& operator-=(const Vec4& v);
    inline Vec3& N();
    inline const Vec3& N() const;
    inline bool Equal(const Vec4& v) const;
    inline bool NotEqual(const Vec4& v) const;
    
    inline float Eval(const Vec3& point) const;
    inline bool Coplanar(const Vec4& v) const;
    inline bool RayIntersect(Vec3& out, const Vec3& org, const Vec3& dir) const;
    inline bool LineIntersect(Vec3& out, const Vec3& beg, const Vec3& end) const;
    inline Vec3 MirrorPoint(const Vec3& point) const;
    inline Vec3 MirrorVector(const Vec3& point) const;
    inline Vec4 MirrorPlane(const Vec4& plane) const;
    inline Vec3 ProjectPoint(const Vec3& point) const;
    inline bool PointOnPlane(const Vec3& point) const;

    friend Vec4 operator*(const Vec4& a, const Vec4& b);
    friend Vec4 operator/(const Vec4& a, const Vec4& b);
    friend Vec4 operator+(const Vec4& a, const Vec4& b);
    friend Vec4 operator+(const Vec4& v);
    friend Vec4 operator-(const Vec4& a, const Vec4& b);
    friend Vec4 operator-(const Vec4& v);
    friend bool operator==(const Vec4& a, const Vec4& b);
    friend bool operator!=(const Vec4& a, const Vec4& b);
    friend bool PlanesIntersect(Vec3& out, const Vec4& a, const Vec4& b, const Vec4& c);
    friend Vec4 PlaneFromPoint(const Vec3& point, const Vec3& normal);
    friend Vec4 PlaneFromPoints(const Vec3& a, const Vec3& b, const Vec3& c);
    friend Vec4 PlaneFromEdge(const Vec3& begin, const Vec3& end, const Vec3& normal);
    friend Vec4 Lerp(const Vec4& a, const Vec4& b, float f);

};

// ========================================================================
// Quaternion

struct Quat
{
    float x;
    float y;
    float z;
    float w;

    inline Quat();
    inline Quat(const float* q);
    inline Quat(const Quat& q);
    inline Quat(const Vec3& v, float w);
    inline Quat(float x, float y, float z, float w);
    inline Quat(float f);

    inline operator float*();
    inline operator const float*() const;
    inline operator Vec4() const;
    inline float& operator[](int n);
    inline Quat& operator=(const Quat& q);

    inline Quat& operator*=(const Quat& q);
    inline Quat& operator*=(const float f);
    inline Quat& operator+=(const Quat& q);
    inline Quat& operator-=(const Quat& q);

    inline Vec3& N();
    inline const Vec3& N() const;

    inline Vec3 TransformVector(const Vec3& v) const;

    friend Quat operator*(const Quat& a, const Quat& b);
    friend Quat operator*(const Quat& q, float f);
    friend Quat operator*(float f, const Quat& q);
    friend Quat operator+(const Quat& a, const Quat& b);
    friend Quat operator+(const Quat& q);
    friend Quat operator-(const Quat& a, const Quat& b);
    friend Quat operator-(const Quat& q);
    friend bool operator==(const Quat& a, const Quat& b);
    friend bool operator!=(const Quat& a, const Quat& b);

    friend Quat Normalize(const Quat& q);
    friend float Dot(const Quat& a, const Quat& b);
    friend Quat Slerp(const Quat& a, const Quat& b, float f, bool shortestPath);
    friend Quat Inverse(const Quat& q);
    friend Quat UnitInverse(const Quat& q);
    friend Quat QuaternionFromAngleAxis(const Vec3& axis, float angle);
    friend Quat QuaternionRotationYawPitchRoll(float yaw, float pitch, float roll);
    friend Quat QuaternionRotationYawPitchRoll(const Vec3& angles);

};

// ========================================================================
// Matrices

struct Matrix3
{
    float _11;
    float _12;
    float _13;
    float _21;
    float _22;
    float _23;
    float _31;
    float _32;
    float _33;

    inline Matrix3();
    inline Matrix3(const float* m);
    inline Matrix3(const Matrix3& m);
    inline Matrix3(float _11, float _12, float _13, float _21, float _22, float _23, float _31, float _32, float _33);
    inline Matrix3(float f);
    inline operator float*();
    inline operator const float*() const;
    inline operator Matrix4() const;
    inline Vec3& operator[](int n);
    inline float& operator()(int n, int m);
    inline Matrix3& operator=(const Matrix3& m);

    inline Matrix3& operator*=(const Matrix3& m);

    inline float Det() const;
    inline Vec2 TransformPoint(const Vec2& tc) const;

    friend Matrix3 operator*(const Matrix3& a, const Matrix3& b);
    friend float Det(const Matrix3& m);
    friend Matrix3 MatrixTextureRotation(float angle);
    friend Matrix3 MatrixTextureTranslation(float u, float v);
    friend Matrix3 MatrixTextureTranslation(const Vec2& v);
    friend Matrix3 MatrixTextureScaling(float u, float v);
    friend Matrix3 MatrixTextureScaling(const Vec2& v);

};

struct Matrix4
{

    float _11;
    float _12;
    float _13;
    float _14;
    float _21;
    float _22;
    float _23;
    float _24;
    float _31;
    float _32;
    float _33;
    float _34;
    float _41;
    float _42;
    float _43;
    float _44;

    inline Matrix4();
    inline Matrix4(const float* m);
    inline Matrix4(const Matrix4& m);
    inline Matrix4(float _11, float _12, float _13, float _14, float _21, float _22, float _23, float _24, float _31, float _32, float _33, float _34, float _41, float _42, float _43, float _44);
    inline Matrix4(float f);
    inline operator float*();
    inline operator const float*() const;
    inline operator Matrix3() const;
    inline Vec4& operator[](int n);
    inline float& operator()(int n, int m);
    inline Matrix4& operator=(const Matrix4& m);

    inline Matrix4& operator*=(const Matrix4& a);

    inline float Det() const;
    inline Matrix4& Inverse();
    inline Matrix4& Transpose();

    inline Vec3 TransformPoint(const Vec3& point) const;
    inline Vec3 TransformVector(const Vec3& vector) const;

    friend Matrix4 operator*(const Matrix4& a, const Matrix4& b);
    friend Matrix4 MatrixTranslation(float x, float y, float z);
    friend Matrix4 MatrixTranslation(const Vec3& v);
    friend Matrix4 MatrixRotationX(float angle);
    friend Matrix4 MatrixRotationY(float angle);
    friend Matrix4 MatrixRotationZ(float angle);
    friend Matrix4 MatrixRotationQuaternion(const Quat& quat);
    friend Matrix4 MatrixRotationYawPitchRoll(float yaw, float pitch, float roll);
    friend Matrix4 MatrixRotationYawPitchRoll(const Vec3& angles);
    friend Matrix4 MatrixScaling(float x, float y, float z);
    friend Matrix4 MatrixScaling(const Vec3& v);
    friend Matrix4 MatrixPerspectiveFovXRH(float fovx, float aspect, float znear, float zfar);
    friend Matrix4 MatrixPerspectiveFovYRH(float fovy, float aspect, float znear, float zfar);
    friend Matrix4 MatrixOrthoRH(float w, float h, float zn, float zf);
    friend Matrix4 MatrixOrthoLH(float w, float h, float zn, float zf);
    friend Matrix4 MatrixLookAtRH(const Vec3& eye, const Vec3& at, const Vec3& up);
    friend float Det(const Matrix4& m);
    friend Matrix4 Inverse(const Matrix4& m);
    friend Matrix4 Transpose(const Matrix4& m);

};

// ========================================================================

struct Color
{
    unsigned char blue;
    unsigned char green;
    unsigned char red;
    unsigned char alpha;

    inline Color();
    inline Color(const Vec3& v);
    inline Color(const Vec4& v);
    inline Color(float r, float g, float b, float a = 1.0f);
    inline Color(int r, int g, int b, int a = 255);
    inline Color(unsigned char r, unsigned char g, unsigned char b, unsigned char a = 255);
    inline Color(const Color& c);
    inline Color(unsigned long c);
    inline Color(long c);
    inline Color(unsigned int c);
    inline Color(int c);
    inline Color(float c);

    inline operator Vec3() const;
    inline operator Vec4() const;
    inline operator unsigned char*() const;

    inline Color& operator*=(Color c);

    inline unsigned long ARGB() const;

    inline unsigned long& ARGB();

    inline float Redf() const;
    inline float Greenf() const;
    inline float Bluef() const;
    inline float Alphaf() const;

    friend Color operator*(Color a, Color b);

};

// ========================================================================
// Various geometric functions

inline bool PointInTriangle(const Vec3& point, const Vec3& a, const Vec3& b, const Vec3& c);
inline void LinesNearestPoints(Vec3 out1, Vec3 out2, const Vec3& org1, const Vec3& dir1, const Vec3& org2, const Vec3& dir2);
inline bool TriangleSameWinding(const Vec3& a1, const Vec3& a2, const Vec3& a3, const Vec3& b1, const Vec3& b2, const Vec3& b3);
inline bool TriangleWindingCheck(const Vec3& a1, const Vec3& a2, const Vec3& a3, const Vec3& normal);
inline float PointRayDist(const Vec3& point, const Vec3& org, const Vec3& dir);
inline float PointSegmentDist(const Vec3& point, const Vec3& a, const Vec3& b);
inline float RaySegmentDist(const Vec3& org, const Vec3& dir, const Vec3& a, const Vec3& b);
inline bool LineShapeIntersect(Vec3& intersect, const Vec3& beg, const Vec3& end, const Vec4& shapePlane, const Vec3* shape, int shapeCount);
inline bool PointInPoly(const Vec3& pnt, const Vec3* shape, int length, const Vec3& normal);
inline float PointShapeDist(const Vec3& point, const Vec3* shape, int length, const Vec4& plane);
inline bool PointsAreCollinear(const Vec3& a, const Vec3& b, const Vec3& c);

// ========================================================================
// Implementations starts here
// ========================================================================
// Vec2 methods

inline Vec2::Vec2() {
}

inline Vec2::Vec2(const float* v) {
    x = v[0]; y = v[1];
}

inline Vec2::Vec2(const Vec2& v) {
    x = v.x; y = v.y;
}

inline Vec2::Vec2(float x, float y) {
    this->x = x; this->y = y;
}

inline Vec2::Vec2(float f) {
    x = y = f;
}

inline Vec2::operator float*() {
    return (float*)this;
}

inline Vec2::operator const float*() const {
    return (const float*)this;
}

inline Vec2::operator Vec3() const {
    return Vec3(x, y, 0);
}

inline Vec2::operator Vec4() const {
    return Vec4(x, y, 0, 0);
}

inline float& Vec2::operator[](int n) {
    assert(n >= 0 && n < 2);
    return *((float*)this + n);
}

inline Vec2& Vec2::operator=(const Vec2& v) {
    x = v.x; y = v.y;
    return *this;
}

inline Vec2& Vec2::operator*=(const Vec2& v) {
    x *= v.x; y *= v.y;
    return *this;
}

inline Vec2& Vec2::operator/=(const Vec2& v) {
    x /= v.x; y /= v.y;
    return *this;
}

inline Vec2& Vec2::operator+=(const Vec2& v) {
    x += v.x; y += v.y;
    return *this;
}

inline Vec2& Vec2::operator-=(const Vec2& v) {
    x -= v.x; y -= v.y;
    return *this;
}

inline Vec2& Vec2::Normalize() {
    *this /= Length();
    return *this;
}

inline float Vec2::Length() const {
    return (float)sqrt(x * x + y * y);
}

inline bool Vec2::Equal(const Vec2& v) const {
    return fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON;
}

inline bool Vec2::NotEqual(const Vec2& v) const {
    return fabs(x - v.x) >= EPSILON && fabs(y - v.y) >= EPSILON;
}

inline Vec2 operator*(const Vec2& a, const Vec2& b) {
    return Vec2(a.x * b.x, a.y * b.y);
}

inline Vec2 operator/(const Vec2& a, const Vec2& b) {
    return Vec2(a.x / b.x, a.y / b.y);
}

inline Vec2 operator+(const Vec2& a, const Vec2& b) {
    return Vec2(a.x + b.x, a.y + b.y);
}

inline Vec2 operator+(const Vec2& v) {
    return v;
}

inline Vec2 operator-(const Vec2& a, const Vec2& b) {
    return Vec2(a.x - b.x, a.y - b.y);
}

inline Vec2 operator-(const Vec2& v) {
    return Vec2(-v.x, -v.y);
}

inline bool operator==(const Vec2& a, const Vec2& b) {
    return a.x == b.x && a.y == b.y;
}

inline bool operator!=(const Vec2& a, const Vec2& b) {
    return a.x != b.x || a.y != b.y;
}

inline Vec2 Perpendicular(const Vec2& v) {
    return Vec2(-v.y, v.x);
}

inline float Dot(const Vec2& a, const Vec2& b) {
    return a.x * b.x + a.y * b.y;
}

inline Vec2 Normalize(const Vec2& v) {
    return v / v.Length();
}

inline Vec2 Lerp(const Vec2& a, const Vec2& b, float f) {
    return Vec2(a.x * (1.0f - f) + b.x * f, a.y * (1.0f - f) + b.y * f);
}

inline float Length(const Vec2& a) {
    return a.Length();
}

// ========================================================================
// Vec3 methods

inline Vec3::Vec3() {
}

inline Vec3::Vec3(const float* v) {
    x = v[0]; y = v[1]; z = v[2];
}

inline Vec3::Vec3(const Vec3& v) {
    x = v.x; y = v.y; z = v.z;
}

inline Vec3::Vec3(float x, float y, float z) {
    this->x = x; this->y = y; this->z = z;
}

inline Vec3::Vec3(float f) {
    x = y = z = f;
}

inline Vec3::operator float*() {
    return (float*)this;
}

inline Vec3::operator const float*() const {
    return (const float*)this;
}

inline Vec3::operator Vec2() const {
    return Vec2(x, y);
}

inline Vec3::operator Vec4() const {
    return Vec4(x, y, z, 0);
}

inline float& Vec3::operator[](int n) {
    assert(n >= 0 && n < 3);
    return ((float*)this)[n];
}

inline Vec3& Vec3::operator=(const Vec3& v) {
    x = v.x; y = v.y; z = v.z;
    return *this;
}

inline Vec3& Vec3::Normalize() {
    *this /= Length();
    return *this;
}

inline float Vec3::Length() const {
    return (float)sqrt(x * x + y * y + z * z);
}

inline bool Vec3::Equal(const Vec3& v) const {
    return (float)fabs(x - v.x) < EPSILON && (float)fabs(y - v.y) < EPSILON && (float)fabs(z - v.z) < EPSILON;
}

inline bool Vec3::NotEqual(const Vec3& v) const {
    return (float)fabs(x - v.x) >= EPSILON || (float)fabs(y - v.y) >= EPSILON || (float)fabs(z - v.z) >= EPSILON;
}

inline Vec3 Vec3::MakePlanar(const Vec3& v) const {
    return v - *this * Dot(*this, v);
}

inline Vec3& Vec3::operator*=(const Vec3& v) {
    x *= v.x; y *= v.y; z *= v.z;
    return *this;
}

inline Vec3& Vec3::operator/=(const Vec3& v) {
    x /= v.x; y /= v.y; z /= v.z;
    return *this;
}

inline Vec3& Vec3::operator+=(const Vec3& v) {
    x += v.x; y += v.y; z += v.z;
    return *this;
}

inline Vec3& Vec3::operator-=(const Vec3& v) {
    x -= v.x; y -= v.y; z -= v.z;
    return *this;
}

inline Vec3 operator*(const Vec3& a, const Vec3& b) {
    return Vec3(a.x * b.x, a.y * b.y, a.z * b.z);
}

inline Vec3 operator/(const Vec3& a, const Vec3& b) {
    return Vec3(a.x / b.x, a.y / b.y, a.z / b.z);
}

inline Vec3 operator/(float f, const Vec3& v) {
    return Vec3(f / v.x, f / v.y, f / v.z);
}

inline Vec3 operator+(const Vec3& a, const Vec3& b) {
    return Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
}

inline Vec3 operator+(const Vec3& v) {
    return v;
}

inline Vec3 operator-(const Vec3& a, const Vec3& b) {
    return Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
}

inline Vec3 operator-(const Vec3& v) {
    return Vec3(-v.x, -v.y, -v.z);
}

inline bool operator==(const Vec3& a, const Vec3& b) {
    return a.x == b.x && a.y == b.y && a.z == b.z;
}

inline bool operator!=(const Vec3& a, const Vec3& b) {
    return a.x != b.x || a.y != b.y || a.z != b.z;
}

inline bool operator<(const Vec3& a, const Vec3& b) {
    return a.x < b.x || a.y < b.y || a.z < b.z;
}

inline Vec3 Cross(const Vec3& a, const Vec3& b) {
    return Vec3(a.y * b.z - a.z * b.y,
                a.z * b.x - a.x * b.z,
                a.x * b.y - a.y * b.x);
}

inline float Dot(const Vec3& a, const Vec3& b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

inline Vec3 Normalize(const Vec3& v) {
    return v / v.Length();
}

inline Vec3 Lerp(const Vec3& a, const Vec3& b, float f) {
    return Vec3(a.x * (1.0f - f) + b.x * f, a.y * (1.0f - f) + b.y * f, a.z * (1.0f - f) + b.z * f);
}

inline float Length(const Vec3& a) {
    return a.Length();
}

inline Vec3 RotateVectorX(const Vec3& v, float angle) {
    return Vec3(v.x,
                v.y * (float)cos(angle) - v.z * (float)sin(angle),
                v.y * (float)sin(angle) + v.z * (float)cos(angle));
}

inline Vec3 RotateVectorY(const Vec3& v, float angle) {
    return Vec3(v.x * (float)cos(angle) + v.z * (float)sin(angle),
                v.y,
                -v.x * (float)sin(angle) + v.z * (float)cos(angle));

}

inline Vec3 RotateVectorZ(const Vec3& v, float angle) {
    return Vec3(v.x * (float)cos(angle) - v.y * (float)sin(angle),
                v.x * (float)sin(angle) + v.y * (float)cos(angle),
                v.z);
}

// ========================================================================
// Vec4 methods

inline Vec4::Vec4() {
}

inline Vec4::Vec4(const float* v) {
    x = v[0]; y = v[1]; z = v[2]; w = v[3];
}

inline Vec4::Vec4(const Vec4& v) {
    x = v.x; y = v.y; z = v.z; w = v.w;
}

inline Vec4::Vec4(const Vec3& v, float w) {
    x = v.x; y = v.y; z = v.z; this->w = w;
}

inline Vec4::Vec4(float x, float y, float z, float w) {
    this->x = x; this->y = y; this->z = z; this->w = w;
}

inline Vec4::Vec4(float f) {
    x = y = z = w = f;
}

inline Vec4::operator float*() {
    return (float*)this;
}

inline Vec4::operator const float*() const {
    return (const float*)this;
}

inline Vec4::operator Vec3() const {
    return Vec3(x, y, z);
}

inline Vec4::operator const Vec3&() const {
    return *(Vec3*)this;
}

inline float& Vec4::operator[](int n) {
    assert(n >= 0 && n < 4);
    return *((float*)this + n);
}

inline Vec4& Vec4::operator=(const Vec4& v) {
    x = v.x; y = v.y; z = v.z; w = v.w;
    return *this;
}

inline Vec4& Vec4::operator*=(const Vec4& v) {
    x *= v.x; y *= v.y; z *= v.z; w *= v.w;
    return *this;
}

inline Vec4& Vec4::operator/=(const Vec4& v) {
    x /= v.x; y /= v.y; z /= v.z; w /= v.w;
    return *this;
}

inline Vec4& Vec4::operator+=(const Vec4& v) {
    x += v.x; y += v.y; z += v.z; w += v.w;
    return *this;
}

inline Vec4& Vec4::operator-=(const Vec4& v) {
    x -= v.x; y -= v.y; z -= v.z; w -= v.w;
    return *this;
}

inline Vec3& Vec4::N() {
    return *(Vec3*)this;
}

inline const Vec3& Vec4::N() const {
    return *(const Vec3*)this;
}

inline bool Vec4::Equal(const Vec4& v) const {
    return (float)fabs(x - v.x) < EPSILON && (float)fabs(y - v.y) < EPSILON && (float)fabs(z - v.z) < EPSILON && (float)fabs(w - v.w) < EPSILON;
}

inline bool Vec4::NotEqual(const Vec4& v) const {
    return (float)fabs(x - v.x) >= EPSILON || (float)fabs(y - v.y) >= EPSILON || (float)fabs(z - v.z) >= EPSILON || (float)fabs(w - v.w) >= EPSILON;
}

inline float Vec4::Eval(const Vec3& point) const {
    return Dot((Vec3&)*this, point) + w;
}

inline bool Vec4::Coplanar(const Vec4& v) const {
    return Equal(v) || Equal(-v);
}

inline bool Vec4::RayIntersect(Vec3& out, const Vec3& org, const Vec3& dir) const {
    float   b, e;
    Vec3    end;

    if(fabs(Dot(dir, (Vec3&)*this)) < EPSILON)
        return false;

    end = org + dir;

    b = Eval(org);
    e = Eval(end);
    out = (org * e - end * b) / (e - b);
    return true;
}

inline bool Vec4::LineIntersect(Vec3& out, const Vec3& beg, const Vec3& end) const {
    float       e, b;

    b = Eval(beg);
    e = Eval(end);

    if(fabs(e - b) < EPSILON)
        return false;

    out = (beg * e - end * b) / (e - b);
    return true;
}

inline Vec3 Vec4::MirrorPoint(const Vec3& point) const {
    return point + N() * -2.0f * Eval(point);
}

inline Vec3 Vec4::MirrorVector(const Vec3& point) const {
    return point + N() * -2.0f * Dot(N(), point);
}

inline Vec4 Vec4::MirrorPlane(const Vec4& plane) const {
    return PlaneFromPoint(MirrorPoint(plane.N() * plane.w), MirrorVector(plane.N()));
}

inline Vec3 Vec4::ProjectPoint(const Vec3& point) const {
    return point - Eval(point) * N();
}

inline bool Vec4::PointOnPlane(const Vec3& point) const {
    return fabs(Eval(point)) < EPSILON;
}

inline Vec4 operator*(const Vec4& a, const Vec4& b) {
    return Vec4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
}

inline Vec4 operator/(const Vec4& a, const Vec4& b) {
    return Vec4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w);
}

inline Vec4 operator+(const Vec4& a, const Vec4& b) {
    return Vec4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
}

inline Vec4 operator+(const Vec4& v) {
    return v;
}

inline Vec4 operator-(const Vec4& a, const Vec4& b) {
    return Vec4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
}

inline Vec4 operator-(const Vec4& v) {
    return Vec4(-v.x, -v.y, -v.z, -v.w);
}

inline bool operator==(const Vec4& a, const Vec4& b) {
    return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
}

inline bool operator!=(const Vec4& a, const Vec4& b) {
    return a.x != b.x || a.y != b.y || a.z != b.z || a.w != b.w;
}

inline bool PlanesIntersect(Vec3& out, const Vec4& a, const Vec4& b, const Vec4& c) {
    float   det, det_k;
    det = a[0] * b[1] * c[2] + a[1] * b[2] * c[0] + a[2] * b[0] * c[1] -
        (c[0] * b[1] * a[2] + c[1] * b[2] * a[0] + c[2] * b[0] * a[1]);
    if( (float)fabs(det) < EPSILON )
        return false;
    det_k = (-a[3]) * b[1] * c[2] + a[1] * b[2] * (-c[3]) + a[2] * (-b[3]) * c[1] -
        ((-c[3]) * b[1] * a[2] + c[1] * b[2] * (-a[3]) + c[2] * (-b[3]) * a[1]);
    out[0] = det_k / det;
    det_k = a[0] * (-b[3]) * c[2] + (-a[3]) * b[2] * c[0] + a[2] * b[0] * (-c[3]) -
        (c[0] * (-b[3]) * a[2] + (-c[3]) * b[2] * a[0] + c[2] * b[0] * (-a[3]));
    out[1] = det_k / det;
    det_k = a[0] * b[1] * (-c[3]) + a[1] * (-b[3]) * c[0] + (-a[3]) * b[0] * c[1] -
        (c[0] * b[1] * (-a[3]) + c[1] * (-b[3]) * a[0] + (-c[3]) * b[0] * a[1]);
    out[2] = det_k / det;
    return true;
}

inline Vec4 PlaneFromPoint(const Vec3& point, const Vec3& normal) {
    return Vec4(normal, -Dot(point, normal));
}

inline Vec4 PlaneFromPoints(const Vec3& a, const Vec3& b, const Vec3& c) {
    return PlaneFromPoint(b, Normalize(Cross(c - b, a - b)));
}

inline Vec4 PlaneFromEdge(const Vec3& begin, const Vec3& end, const Vec3& normal) {
    return PlaneFromPoints(begin, end, end + normal);
}

inline Vec4 Lerp(const Vec4& a, const Vec4& b, float f) {
    return Vec4(a.x * (1.0f - f) + b.x * f, a.y * (1.0f - f) + b.y * f, a.z * (1.0f - f) + b.z * f, a.w * (1.0f - f) + b.w * f);
}

// ========================================================================
// Quat

inline Quat::Quat() {
}

inline Quat::Quat(const float* q) {
    x = q[0]; y = q[1]; z = q[2]; w = q[3];
}

inline Quat::Quat(const Quat& q) {
    x = q.x; y = q.y; z = q.z; w = q.w;
}

inline Quat::Quat(const Vec3& v, float w) {
    x = v.x; y = v.y; z = v.z; this->w = w;
}

inline Quat::Quat(float x, float y, float z, float w) {
    this->x = x; this->y = y; this->z = z; this->w = w;
}

inline Quat::Quat(float f) {
    x = y = z = 0.0f; w = f;
}

inline Quat::operator float*() {
    return (float*)this;
}

inline Quat::operator const float*() const {
    return (const float*)this;
}

inline Quat::operator Vec4() const {
    return Vec4(x, y, z, w);
}

inline float& Quat::operator[](int n) {
    assert(n >= 0 && n < 4);
    return *((float*)this + n);
}

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) {
    *this = *this * q;
    return *this;
}

inline Quat& Quat::operator*=(float f) {
    x *= f; y *= f; z *= f; w *= f;
    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;
}

inline Vec3& Quat::N() {
    return *(Vec3*)this;
}

inline const Vec3& Quat::N() const {
    return *(const Vec3*)this;
}

inline Vec3 Quat::TransformVector(const Vec3& v) const {
    float x2 = 2.0f * x, y2 = 2.0f * y, z2 = 2.0f * z;
    float wx = x2 * w, wy = y2 * w, wz = z2 * w;
    float xx = x2 * x, xy = y2 * x, xz = z2 * x;
    float yy = y2 * y, yz = z2 * y, zz = z2 * z;

    return Vec3(
        (1.0f - (yy + zz)) * v.x + (xy - wz) * v.y + (xz + wy) * v.z,
        (xy + wz) * v.x + (1.0f - (xx + zz)) * v.y + (yz - wx) * v.z,
        (xz - wy) * v.x + (yz + wx) * v.y + (1.0f - (xx + yy)) * v.z
    );
}

inline Quat operator*(const Quat& a, const Quat& b) {
    return Quat(
        a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
        a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
        a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
        a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
    );
}

inline Quat operator*(float f, const Quat& q) {
    return Quat(q.x * f, q.y * f, q.z * f, q.w * f);
}

inline Quat operator*(const Quat& q, float f) {
    return Quat(q.x * f, q.y * f, q.z * f, q.w * f);
}

inline Quat operator+(const Quat& a, const Quat& b) {
    return Quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
}

inline Quat operator+(const Quat& q) {
    return q;
}

inline Quat operator-(const Quat& a, const Quat& b) {
    return Quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
}

inline Quat operator-(const Quat& q) {
    return Quat(q.x, q.y, q.z, -q.w);
}

inline bool operator==(const Quat& a, const Quat& b) {
    return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
}

inline bool operator!=(const Quat& a, const Quat& b) {
    return a.x != b.x || a.y != b.y || a.z != b.z || a.w != b.w;
}

inline Quat Normalize(const Quat& q) {
    float len = Dot(q, q);
    float factor = 1.0f / sqrt(len);
    return q * factor;
}

inline float Dot(const Quat& a, const Quat& b) {
    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

inline Quat Slerp(const Quat& a, const Quat& b, float f, bool shortestPath) {
    float c = Dot(a, b);
    float angle;
    if(c > 1.0f) {
        angle = 0.0f;
    } else if(c < -1.0f) {
        angle = PI;
    } else {
        angle = acos(c);
    }
    if(fabs(angle) < EPSILON) {
        return a;
    }
    float s = sin(angle);
    float invs = 1.0f / s;
    float coeff0 = sinf((1.0f - f) * angle) * invs;
    float coeff1 = sinf(f * angle) * invs;
    // Do we need to invert rotation?
    if(c < 0.0f && shortestPath) {
        coeff0 = -coeff0;
        // taking the complement requires renormalisation
        Quat t(coeff0 * a + coeff1 * b);
        return Normalize(t);
    } else {
        return coeff0 * a + coeff1 * b;
    }
}

inline Quat Inverse(const Quat& q) {
    float norm = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
    if(norm > 0.0f) {
        return Quat(-q.x / norm, -q.y / norm, -q.z / norm, q.w / norm);
    } else {
        return Quat(0.0f);
    }
}

inline Quat UnitInverse(const Quat& q) {
    return Quat(-q.x, -q.y, -q.z, q.w);
}

inline Quat QuaternionFromAngleAxis(const Vec3& axis, float angle) {
    // assert: axis.Length() == 1.0f
    float s = (float)sin(0.5f * angle);
    return Quat(axis.x * s, axis.y * s, axis.z * s, (float)cos(0.5f * angle));
}

inline Quat QuaternionRotationYawPitchRoll(float yaw, float pitch, float roll) {
    float sinY, cosY, sinP, cosP, sinR, cosR;

    sinY = (float)sin(0.5f * yaw);
    cosY = (float)cos(0.5f * yaw);

    sinP = (float)sin(0.5f * pitch);
    cosP = (float)cos(0.5f * pitch);

    sinR = (float)sin(0.5f * roll);
    cosR = (float)cos(0.5f * roll);

    return Quat(cosR * sinP * cosY + sinR * cosP * sinY,
                cosR * cosP * sinY - sinR * sinP * cosY,
                sinR * cosP * cosY - cosR * sinP * sinY,
                cosR * cosP * cosY + sinR * sinP * sinY);
}

inline Quat QuaternionRotationYawPitchRoll(const Vec3& angles) {
    return QuaternionRotationYawPitchRoll(angles.y, angles.x, angles.z);
}

// ========================================================================
// Matrix3

inline Matrix3::Matrix3() {
}

inline Matrix3::Matrix3(const float* m) {
    memcpy(this, m, sizeof(Matrix3));
}

inline Matrix3::Matrix3(const Matrix3& m) {
    memcpy(this, &m, sizeof(Matrix3));
}

inline Matrix3::Matrix3(float _11, float _12, float _13, float _21, float _22, float _23, float _31, float _32, float _33) {
    this->_11 = _11; this->_12 = _12; this->_13 = _13;
    this->_21 = _21; this->_22 = _22; this->_23 = _23;
    this->_31 = _31; this->_32 = _32; this->_33 = _33;
}

inline Matrix3::Matrix3(float f) {
    _11 = f; _12 = 0; _13 = 0;
    _21 = 0; _22 = f; _23 = 0;
    _31 = 0; _32 = 0; _33 = f;
}

inline Matrix3::operator float*() {
    return (float*)this;
}

inline Matrix3::operator const float*() const {
    return (const float*)this;
}

inline Matrix3::operator Matrix4() const {
    return Matrix4(_11, _12, _13, 0, _21, _22, _23, 0, _31, _32, _33, 0, 0, 0, 0, 1);
}

inline Vec3& Matrix3::operator[](int n) {
    assert(n >= 0 && n < 3);
    return *(Vec3*)this;
}

inline float& Matrix3::operator()(int n, int m) {
    assert(n >= 0 && n < 3);
    assert(m >= 0 && m < 3);
    return ((float*)this)[n * 3 + m];
}

inline Matrix3& Matrix3::operator=(const Matrix3& m) {
    memcpy(this, &m, sizeof(Matrix3));
    return *this;
}

inline Matrix3& Matrix3::operator*=(const Matrix3& m) {
    Matrix3 t = *this;
    _11 = t._11 * m._11 + t._12 * m._21 + t._13 * m._31;
    _12 = t._11 * m._12 + t._12 * m._22 + t._13 * m._32;
    _13 = t._11 * m._13 + t._12 * m._23 + t._13 * m._33;

    _21 = t._21 * m._11 + t._22 * m._21 + t._23 * m._31;
    _22 = t._21 * m._12 + t._22 * m._22 + t._23 * m._32;
    _23 = t._21 * m._13 + t._22 * m._23 + t._23 * m._33;

    _31 = t._31 * m._11 + t._32 * m._21 + t._33 * m._31;
    _32 = t._31 * m._12 + t._32 * m._22 + t._33 * m._32;
    _33 = t._31 * m._13 + t._32 * m._23 + t._33 * m._33;

    return *this;
}

inline float Matrix3::Det() const {
    return _11 * (_22 * _33 - _23 * _32) - 
            _21 * (_12 * _33 - _13 * _32) +
            _31 * (_12 * _23 - _13 * _22);
}

inline Vec2 Matrix3::TransformPoint(const Vec2& tc) const {
    return Vec2(tc.x * _11 + tc.y * _21 + _31, tc.x * _12 + tc.y * _22 + _32);
}

inline Matrix3 operator*(const Matrix3& a, const Matrix3& b) {
    Matrix3 r = a;
    r *= b;
    return r;
}

inline float Det(const Matrix3& m) {
    return m.Det();
}

inline Matrix3 MatrixTextureRotation(float angle) {
    float c = (float)cos(angle), s = (float)sin(angle);
    return Matrix3(c, s, 0,
                    -s, c, 0,
                    0, 0, 1);
}

inline Matrix3 MatrixTextureTranslation(float u, float v) {
    return Matrix3(1, 0, 0,
                    0, 1, 0,
                    u, v, 1);
}

inline Matrix3 MatrixTextureTranslation(const Vec2& v) {
    return Matrix3(1, 0, 0,
                    0, 1, 0,
                    v.x, v.y, 1);
}

inline Matrix3 MatrixTextureScaling(float u, float v) {
    return Matrix3(u, 0, 0,
                    0, v, 0,
                    0, 0, 1);
}

inline Matrix3 MatrixTextureScaling(const Vec2& v) {
    return Matrix3(v.x, 0, 0,
                    0, v.y, 0,
                    0, 0, 1);
}

// ========================================================================

inline Matrix4::Matrix4() {
}

inline Matrix4::Matrix4(const float* m) {
    memcpy(this, m, sizeof(Matrix4));
}

inline Matrix4::Matrix4(const Matrix4& m) {
    memcpy(this, &m, sizeof(Matrix4));
}

inline Matrix4::Matrix4(float _11, float _12, float _13, float _14, float _21, float _22, float _23, float _24, float _31, float _32, float _33, float _34, float _41, float _42, float _43, float _44) {
    this->_11 = _11; this->_12 = _12; this->_13 = _13; this->_14 = _14;
    this->_21 = _21; this->_22 = _22; this->_23 = _23; this->_24 = _24;
    this->_31 = _31; this->_32 = _32; this->_33 = _33; this->_34 = _34;
    this->_41 = _41; this->_42 = _42; this->_43 = _43; this->_44 = _44;
}

inline Matrix4::Matrix4(float f) {
    _11 = f; _12 = 0; _13 = 0; _14 = 0;
    _21 = 0; _22 = f; _23 = 0; _24 = 0;
    _31 = 0; _32 = 0; _33 = f; _34 = 0;
    _41 = 0; _42 = 0; _43 = 0; _44 = f;
}

inline Matrix4::operator float*() {
    return (float*)this;
}

inline Matrix4::operator const float*() const {
    return (const float*)this;
}

inline Matrix4::operator Matrix3() const {
    return Matrix3(_11, _12, _13, _21, _22, _23, _31, _32, _33);
}

inline Vec4& Matrix4::operator[](int n) {
    assert(n >= 0 && n < 4);
    return *((Vec4*)this + n);
}

inline float& Matrix4::operator()(int n, int m) {
    assert(n >= 0 && n < 4);
    assert(m >= 0 && m < 4);
    return ((float*)this)[n * 4 + m];
}

inline Matrix4& Matrix4::operator=(const Matrix4& m) {
    memcpy(this, &m, sizeof(Matrix4));
    return *this;
}

inline Matrix4& Matrix4::operator*=(const Matrix4& a) {
    Matrix4 t = *this;
    _11 = t._11 * a._11 + t._12 * a._21 + t._13 * a._31 + t._14 * a._41;
    _12 = t._11 * a._12 + t._12 * a._22 + t._13 * a._32 + t._14 * a._42;
    _13 = t._11 * a._13 + t._12 * a._23 + t._13 * a._33 + t._14 * a._43;
    _14 = t._11 * a._14 + t._12 * a._24 + t._13 * a._34 + t._14 * a._44;

    _21 = t._21 * a._11 + t._22 * a._21 + t._23 * a._31 + t._24 * a._41;
    _22 = t._21 * a._12 + t._22 * a._22 + t._23 * a._32 + t._24 * a._42;
    _23 = t._21 * a._13 + t._22 * a._23 + t._23 * a._33 + t._24 * a._43;
    _24 = t._21 * a._14 + t._22 * a._24 + t._23 * a._34 + t._24 * a._44;

    _31 = t._31 * a._11 + t._32 * a._21 + t._33 * a._31 + t._34 * a._41;
    _32 = t._31 * a._12 + t._32 * a._22 + t._33 * a._32 + t._34 * a._42;
    _33 = t._31 * a._13 + t._32 * a._23 + t._33 * a._33 + t._34 * a._43;
    _34 = t._31 * a._14 + t._32 * a._24 + t._33 * a._34 + t._34 * a._44;

    _41 = t._41 * a._11 + t._42 * a._21 + t._43 * a._31 + t._44 * a._41;
    _42 = t._41 * a._12 + t._42 * a._22 + t._43 * a._32 + t._44 * a._42;
    _43 = t._41 * a._13 + t._42 * a._23 + t._43 * a._33 + t._44 * a._43;
    _44 = t._41 * a._14 + t._42 * a._24 + t._43 * a._34 + t._44 * a._44;

    return *this;
}

inline float Matrix4::Det() const {
    return _11 * GLFX::Det(Matrix3(_22, _23, _24, _32, _33, _34, _42, _43, _44)) -
            _21 * GLFX::Det(Matrix3(_12, _13, _14, _32, _33, _34, _42, _43, _44)) +
            _31 * GLFX::Det(Matrix3(_12, _13, _14, _22, _23, _24, _42, _43, _44)) -
            _41 * GLFX::Det(Matrix3(_12, _13, _14, _22, _23, _24, _32, _33, _34));
}

inline Matrix4& Matrix4::Inverse() {
    Matrix4 t = *this;
    float det = Det();

    _11 = GLFX::Det(Matrix3(t._22, t._23, t._24, t._32, t._33, t._34, t._42, t._43, t._44));
    _12 = -GLFX::Det(Matrix3(t._12, t._13, t._14, t._32, t._33, t._34, t._42, t._43, t._44));
    _13 = GLFX::Det(Matrix3(t._12, t._13, t._14, t._22, t._23, t._24, t._42, t._43, t._44));
    _14 = -GLFX::Det(Matrix3(t._12, t._13, t._14, t._22, t._23, t._24, t._32, t._33, t._34));

    _21 = -GLFX::Det(Matrix3(t._21, t._23, t._24, t._31, t._33, t._34, t._41, t._43, t._44));
    _22 = GLFX::Det(Matrix3(t._11, t._13, t._14, t._31, t._33, t._34, t._41, t._43, t._44));
    _23 = -GLFX::Det(Matrix3(t._11, t._13, t._14, t._21, t._23, t._24, t._41, t._43, t._44));
    _24 = GLFX::Det(Matrix3(t._11, t._13, t._14, t._21, t._23, t._24, t._31, t._33, t._34));

    _31 = GLFX::Det(Matrix3(t._21, t._22, t._24, t._31, t._32, t._34, t._41, t._42, t._44));
    _32 = -GLFX::Det(Matrix3(t._11, t._12, t._14, t._31, t._32, t._34, t._41, t._42, t._44));
    _33 = GLFX::Det(Matrix3(t._11, t._12, t._14, t._21, t._22, t._24, t._41, t._42, t._44));
    _34 = -GLFX::Det(Matrix3(t._11, t._12, t._14, t._21, t._22, t._24, t._31, t._32, t._34));

    _41 = -GLFX::Det(Matrix3(t._21, t._22, t._23, t._31, t._32, t._33, t._41, t._42, t._43));
    _42 = GLFX::Det(Matrix3(t._11, t._12, t._13, t._31, t._32, t._33, t._41, t._42, t._43));
    _43 = -GLFX::Det(Matrix3(t._11, t._12, t._13, t._21, t._22, t._23, t._41, t._42, t._43));
    _44 = GLFX::Det(Matrix3(t._11, t._12, t._13, t._21, t._22, t._23, t._31, t._32, t._33));

    if(!det)
        return *this;       // singular matrix, no inverse

    for(int i = 0; i < 16; i++)
        ((float*)this)[i] /= det;

    return *this;
}

inline Matrix4& Matrix4::Transpose() {
    float t;
    t = _12; _12 = _21; _21 = t;
    t = _13; _13 = _31; _31 = t;
    t = _14; _14 = _41; _41 = t;
    t = _23; _23 = _32; _32 = t;
    t = _24; _24 = _42; _42 = t;
    t = _34; _34 = _43; _43 = t;
    return *this;
}

inline Vec3 Matrix4::TransformPoint(const Vec3& point) const {
    float d = point.x * _14 + point.y * _24 + point.z * _34 + _44;
    return Vec3((point.x * _11 + point.y * _21 + point.z * _31 + _41) / d,
                (point.x * _12 + point.y * _22 + point.z * _32 + _42) / d,
                (point.x * _13 + point.y * _23 + point.z * _33 + _43) / d);
}

inline Vec3 Matrix4::TransformVector(const Vec3& vector) const {
    // TODO: this is not working right
    float d = vector.x * _14 + vector.y * _24 + vector.z * _34 + _44;
    return Vec3((vector.x * _11 + vector.y * _21 + vector.z * _31) / d,
                (vector.x * _12 + vector.y * _22 + vector.z * _32) / d,
                (vector.x * _13 + vector.y * _23 + vector.z * _33) / d);
}

inline Matrix4 operator*(const Matrix4& a, const Matrix4& b) {
    Matrix4 t = a;
    t *= b;
    return t;
}

inline Matrix4 MatrixTranslation(float x, float y, float z) {
    return Matrix4(1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, 0,
                    x, y, z, 1);
}

inline Matrix4 MatrixTranslation(const Vec3& v) {
    return Matrix4(1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, 0,
                    v.x, v.y, v.z, 1);
}

inline Matrix4 MatrixRotationX(float angle) {
    float   c = (float)cos(angle);
    float   s = (float)sin(angle);
    float   n = -s;

    return Matrix4(1, 0, 0, 0,
                    0, c, s, 0,
                    0, n, c, 0,
                    0, 0, 0, 1);
}

inline Matrix4 MatrixRotationY(float angle) {
    float   c = (float)cos(angle);
    float   s = (float)sin(angle);
    float   n = -s;

    return Matrix4(c, 0, n, 0,
                    0, 1, 0, 0,
                    s, 0, c, 0,
                    0, 0, 0, 1);
}

inline Matrix4 MatrixRotationZ(float angle) {
    float   c = (float)cos(angle);
    float   s = (float)sin(angle);
    float   n = -s;

    return Matrix4(c, s, 0, 0,
                    n, c, 0, 0,
                    0, 0, 1, 0,
                    0, 0, 0, 1);
}

inline Matrix4 MatrixRotationQuaternion(const Quat& quat) {
    float x = 2.0f * quat.x, y = 2.0f * quat.y, z = 2.0f * quat.z;
    float wx = x * quat.w, wy = y * quat.w, wz = z * quat.w;
    float xx = x * quat.x, xy = y * quat.x, xz = z * quat.x;
    float yy = y * quat.y, yz = z * quat.y, zz = z * quat.z;

    return Matrix4(1.0f - (yy + zz), xy - wz, xz + wy, 0,
                    xy + wz, 1.0f - (xx + zz), yz - wx, 0,
                    xz - wy, yz + wx, 1.0f - (xx + yy), 0,
                    0, 0, 0, 1);
                    
}

inline Matrix4 MatrixRotationYawPitchRoll(float yaw, float pitch, float roll) {
    return MatrixRotationY(yaw) * MatrixRotationX(pitch) * MatrixRotationZ(roll);
}

inline Matrix4 MatrixRotationYawPitchRoll(const Vec3& angles) {
    return MatrixRotationYawPitchRoll(angles.y, angles.x, angles.z);
}

inline Matrix4 MatrixScaling(float x, float y, float z) {
    return Matrix4(x, 0, 0, 0,
                    0, y, 0, 0,
                    0, 0, z, 0,
                    0, 0, 0, 1);
}

inline Matrix4 MatrixScaling(const Vec3& v) {
    return MatrixScaling(v.x, v.y, v.z);
}

inline Matrix4 MatrixPerspectiveFovXRH(float fovx, float aspect, float znear, float zfar) {
    float   w, h, x, y, c, d;

    w = 2 * znear * (float)tan(fovx * PI / 360);
    h = w / aspect;
    x = 2 * znear / w;
    y = 2 * znear / h;
    c = -(zfar + znear) / (zfar - znear);
    d = -(2 * zfar * znear) / (zfar - znear);

    return Matrix4(x, 0, 0, 0,
                    0, y, 0, 0,
                    0, 0, c, -1,
                    0, 0, d, 0);
}

inline Matrix4 MatrixPerspectiveFovYRH(float fovy, float aspect, float znear, float zfar) {
    float   w, h, x, y, c, d;

    h = 2 * znear * (float)tan(fovy * PI / 360);
    w = h * aspect;
    x = 2 * znear / w;
    y = 2 * znear / h;
    c = -(zfar + znear) / (zfar - znear);
    d = -(2 * zfar * znear) / (zfar - znear);

    return Matrix4(x, 0, 0, 0,
                    0, y, 0, 0,
                    0, 0, c, -1,
                    0, 0, d, 0);
}

inline Matrix4 MatrixOrthoRH(float w, float h, float zn, float zf) {
    float   x, y, z, t;
    x = 2.0f / w;
    y = 2.0f / h;
    z = -1.0f / (zf - zn);
    t = -zn / (zf - zn);

    return Matrix4(x, 0, 0, 0,
                    0, y, 0, 0,
                    0, 0, z, 0,
                    0, 0, t, 1);
}

inline Matrix4 MatrixOrthoLH(float w, float h, float zn, float zf) {
    float   x, y, z, t;
    x = 2 / w;
    y = 2 / h;
    // FIXME: Is this ok?
    z = 1.0f / (zf - zn);
    t = zn / (zf - zn);

    return Matrix4(x, 0, 0, 0,
                    0, y, 0, 0,
                    0, 0, z, 0,
                    0, 0, t, 1);
}

inline Matrix4 MatrixLookAtRH(const Vec3& eye, const Vec3& at, const Vec3& up) {
    Vec3    x, y, z;
    Matrix4 r;

    z = eye - at;
    z = Normalize(z);

    x = Cross(up, z);
    y = Cross(z, x);

    x = Normalize(x);
    y = Normalize(y);

    r(0, 0) = x[0];     r(0, 1) = y[0];     r(0, 2) = z[0];      r(0, 3) = 0;
    r(1, 0) = x[1];     r(1, 1) = y[1];     r(1, 2) = z[1];      r(1, 3) = 0;
    r(2, 0) = x[2];     r(2, 1) = y[2];     r(2, 2) = z[2];      r(2, 3) = 0;
    r(3, 0) = 0;        r(3, 1) = 0;        r(3, 2) = 0;         r(3, 3) = 1;

    return MatrixTranslation(-eye) * r;
}

inline float Det(const Matrix4& m) {
    return m.Det();
}

inline Matrix4 Inverse(const Matrix4& m) {
    Matrix4 t = m;
    return t.Inverse();
}

inline Matrix4 Transpose(const Matrix4& m) {
    Matrix4 t = m;
    return t.Transpose();
}

// ========================================================================

inline Color::Color() {
}

inline Color::Color(const Vec3& v) {
    red = (unsigned char)(v.x * 255.0f);
    green = (unsigned char)(v.y * 255.0f);
    blue = (unsigned char)(v.z * 255.0f);
    alpha = 0xff;
}

inline Color::Color(const Vec4& v) {
    red = (unsigned char)(v.x * 255.0f);
    green = (unsigned char)(v.y * 255.0f);
    blue = (unsigned char)(v.z * 255.0f);
    alpha = (unsigned char)(v.w * 255.0f);
}

inline Color::Color(float r, float g, float b, float a) {
    red = (unsigned char)(r * 255.0f);
    green = (unsigned char)(g * 255.0f);
    blue = (unsigned char)(b * 255.0f);
    alpha = (unsigned char)(a * 255.0f);
}

inline Color::Color(unsigned char r, unsigned char g, unsigned char b, unsigned char a) {
    red = r;
    green = g;
    blue = b;
    alpha = a;
}

inline Color::Color(int r, int g, int b, int a) {
    red = r & 0xff;
    green = g & 0xff;
    blue = b & 0xff;
    alpha = a & 0xff;
}

inline Color::Color(const Color& c) {
    *(unsigned long*)this = *(unsigned long*)&c;
}

inline Color::Color(unsigned long c) {
    *(unsigned long*)this = c;
}

inline Color::Color(long c) {
    *(long*)this = c;
}

inline Color::Color(unsigned int c) {
    *(unsigned int*)this = c;
}

inline Color::Color(int c) {
    *(int*)this = c;
}

inline Color::Color(float c) {
    red = green = blue = alpha = (unsigned char)(c * 255.0f);
}

inline Color::operator Vec3() const {
    return Vec3(Redf(), Greenf(), Bluef());
}

inline Color::operator Vec4() const {
    return Vec4(Redf(), Greenf(), Bluef(), Alphaf());
}

inline Color::operator unsigned char*() const {
    return (unsigned char*)this;
}

inline Color& Color::operator*=(Color c) {
    red = ((unsigned int)red * (unsigned int)c.red) >> 8;
    green = ((unsigned int)green * (unsigned int)c.green) >> 8;
    blue = ((unsigned int)blue * (unsigned int)c.blue) >> 8;
    alpha = ((unsigned int)alpha * (unsigned int)c.alpha) >> 8;
    return *this;
}

inline unsigned long Color::ARGB() const {
    return *(unsigned long*)this;
}

inline unsigned long& Color::ARGB() {
    return *(unsigned long*)this;
}

inline float Color::Redf() const {
    return red / 255.0f;
}

inline float Color::Greenf() const {
    return green / 255.0f;
}

inline float Color::Bluef() const {
    return blue / 255.0f;
}

inline float Color::Alphaf() const {
    return alpha / 255.0f;
}

inline Color operator*(Color a, Color b) {
    Color r = a;
    r *= b;
    return r;
}

// ========================================================================

inline bool PointInTriangle(const Vec3& point, const Vec3& a, const Vec3& b, const Vec3& c) {
    Vec3 normal = Cross(c - b, a - b);
    return PlaneFromEdge(a, b, normal).Eval(point) < EPSILON &&
        PlaneFromEdge(b, c, normal).Eval(point) < EPSILON &&
        PlaneFromEdge(c, a, normal).Eval(point) < EPSILON;
}

inline void LinesNearestPoints(Vec3 out1, Vec3 out2, const Vec3& org1, const Vec3& dir1, const Vec3& org2, const Vec3& dir2) {
    if(dir1.Equal(dir2)) {
        out1 = org1;
        out2 = org2;
        return;
    }
    float   a, b;
    float   t, s;
    a = Dot(dir1, dir2);
    b = Dot(dir1, org2) - Dot(dir1, org1);
    s = (2 * a * Dot(org1, dir1) - 2 * Dot(org1, dir2) + 2 * a * b -
        2 * a * Dot(dir1, org2) - 2 * b * Dot(dir1, dir2) + 2 * Dot(org2, dir2) ) /
        (-2 * (a * a + 1 - 2 * a * Dot(dir1, dir2)));
    t = a * s + b;

    out1 = dir1 * t + org1;
    out2 = dir2 * t + org2;
}

inline bool TriangleSameWinding(const Vec3& a1, const Vec3& a2, const Vec3& a3, const Vec3& b1, const Vec3& b2, const Vec3& b3) {
    return Dot(Cross(a3 - a2, a1 - a2), Cross(b3 - b2, b1 - b2)) > 0;
}

inline bool TriangleWindingCheck(const Vec3& a1, const Vec3& a2, const Vec3& a3, const Vec3& normal) {
    return Dot(Cross(a3 - a2, a1 - a2), normal) > 0;
}

inline float PointRayDist(const Vec3& point, const Vec3& org, const Vec3& dir) {
    return Length(org + dir * (Dot(dir, point) - Dot(dir, org)) - point);
}

inline float PointSegmentDist(const Vec3& point, const Vec3& a, const Vec3& b) {
    Vec4 plane = PlaneFromPoint(a, Normalize(a - b));
    if(plane.Eval(point) > 0.0f)
        return Length(point - a);
    plane = PlaneFromPoint(b, Normalize(b - a));
    if(plane.Eval(point) > 0.0f)
        return Length(point - b);
    return PointRayDist(point, a, plane.N());
}

inline float RaySegmentDist(const Vec3& org, const Vec3& dir, const Vec3& a, const Vec3& b) {
    Vec3 out1, out2;
    Vec4 plane;
    LinesNearestPoints(out1, out2, org, dir, a, Normalize(b - a));
    plane = PlaneFromPoint(a, Normalize(a - b));
    if(plane.Eval(out2) > 0)
        return PointRayDist(a, org, dir);
    plane = PlaneFromPoint(b, Normalize(b - a));
    if(plane.Eval(out2) > 0)
        return PointRayDist(b, org, dir);
    return Length(out1 - out2);
}

inline bool LineShapeIntersect(Vec3& intersect, const Vec3& beg, const Vec3& end, const Vec4& shapePlane, const Vec3* shape, int shapeCount) {
    float   eval = 0;
    if(!shapePlane.LineIntersect(intersect, beg, end))
        return false;
    for(int i = 0; i < shapeCount; i++) {
        Vec4 sidePlane = PlaneFromEdge(shape[i], shape[(i + 1) % shapeCount], (Vec3&)shapePlane);
        if(fabs(eval) < EPSILON) {
            eval = sidePlane.Eval(intersect);
        } else {
            if(eval * sidePlane.Eval(intersect) < -EPSILON)
                return false;
        }
    }
    return true;
}

inline bool PointInPoly(const Vec3& pnt, const Vec3* shape, int length, const Vec3& normal) {
    float eval = 0;
    for(int i = 0; i < length; i++) {
        Vec4 plane = PlaneFromEdge(shape[i], shape[(i + 1) % length], normal);
        if(fabs(eval) < EPSILON) {
            eval = plane.Eval(pnt);
        } else {
            if(eval * plane.Eval(pnt) < EPSILON)
                return false;
        }
    }
    return true;
}

inline float PointShapeDist(const Vec3& point, const Vec3* shape, int length, const Vec4& plane) {
    Vec3 proj = plane.ProjectPoint(point);
    if(PointInPoly(proj, shape, length, (Vec3&)plane)) {
        return Length(proj - point);
    }
    float minDist;
    minDist = PointSegmentDist(point, shape[0], shape[1]);
    for(int i = 1; i < length; i++) {
        float dist = PointSegmentDist(point, shape[i], shape[(i + 1) % length]);
        if(dist < minDist)
            minDist = dist;
    }
    return minDist;
}

inline bool PointsAreCollinear(const Vec3& a, const Vec3& b, const Vec3& c) {
    if(a.Equal(b) || a.Equal(c) || b.Equal(c))
        return true;
    Vec3 u, v;
    u = Normalize(a - b);
    v = Normalize(b - c);
    if(Dot(u, v) < 0)
        return u.Equal(-v);
    return u.Equal(v);
}

// ========================================================================

}

#pragma pack()

#endif // __GLFXVEC_H__

---