moved a few Extras projects into Extras/obsolete.
This commit is contained in:
229
Extras/obsolete/SATConvexCollision/Geometry.cpp
Normal file
229
Extras/obsolete/SATConvexCollision/Geometry.cpp
Normal file
@@ -0,0 +1,229 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Geometry.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
|
||||
///for now this is windows only, Intel SSE SIMD intrinsics
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
|
||||
|
||||
#include "Geometry.h"
|
||||
#include "Maths.h"
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Line
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Ray
|
||||
|
||||
|
||||
// returns false if the lines are parallel
|
||||
// t1 and t2 are set to the times of the nearest points on each line
|
||||
bool Intersect(const Line& la, const Line& lb, float& ta, float& tb)
|
||||
{
|
||||
Vector3 ea = la.m_end - la.m_start;
|
||||
Vector3 eb = lb.m_end - lb.m_start;
|
||||
Vector3 u = la.m_start - lb.m_start;
|
||||
|
||||
float a = Dot(ea, ea);
|
||||
float b = Dot(ea, eb);
|
||||
float c = Dot(eb, eb);
|
||||
float d = Dot(ea, u);
|
||||
float e = Dot(eb, u);
|
||||
|
||||
float det = (a * c - b * b);
|
||||
|
||||
if (Abs(det) < 0.001f)
|
||||
return false;
|
||||
|
||||
float invDet = RcpNr(det);
|
||||
ta = (b * e - c * d) * invDet;
|
||||
tb = (a * e - b * d) * invDet;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool IntersectSegments(const Line& la, const Line& lb, float& ta, float& tb)
|
||||
{
|
||||
Vector3 ea = la.m_end - la.m_start;
|
||||
Vector3 eb = lb.m_end - lb.m_start;
|
||||
Vector3 u = la.m_start - lb.m_start;
|
||||
|
||||
float a = Dot(ea, ea);
|
||||
float b = Dot(ea, eb);
|
||||
float c = Dot(eb, eb);
|
||||
float d = Dot(ea, u);
|
||||
float e = Dot(eb, u);
|
||||
|
||||
float det = (a * c - b * b);
|
||||
|
||||
if (Abs(det) < 0.001f)
|
||||
return false;
|
||||
|
||||
float numa = (b * e - c * d);
|
||||
float numb = (a * e - b * d);
|
||||
|
||||
// clip a
|
||||
float dena = det, denb = det;
|
||||
if (numa < 0.0f)
|
||||
{
|
||||
numa = 0.0f;
|
||||
numb = e;
|
||||
denb = c;
|
||||
}
|
||||
else if (numa > det)
|
||||
{
|
||||
numa = det;
|
||||
numb = e + b;
|
||||
denb = c;
|
||||
}
|
||||
else
|
||||
denb = det;
|
||||
|
||||
// clip b
|
||||
if (numb < 0.0f)
|
||||
{
|
||||
numb = 0.0f;
|
||||
if (-d < 0.0f)
|
||||
{
|
||||
numa = 0.0f;
|
||||
}
|
||||
else if (-d > a)
|
||||
{
|
||||
numa = dena;
|
||||
}
|
||||
else
|
||||
{
|
||||
numa = -d;
|
||||
dena = a;
|
||||
}
|
||||
}
|
||||
else if (numb > denb)
|
||||
{
|
||||
numb = denb;
|
||||
if ((-d + b) < 0.0f)
|
||||
{
|
||||
numa = 0.0f;
|
||||
}
|
||||
else if ((-d + b) > a)
|
||||
{
|
||||
numa = dena;
|
||||
}
|
||||
else
|
||||
{
|
||||
numa = -d + b;
|
||||
dena = a;
|
||||
}
|
||||
}
|
||||
|
||||
// compute the times
|
||||
ta = numa / dena;
|
||||
tb = numb / denb;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns intersection of 2 rays or nearest point to it
|
||||
// t1 and t2 are set to the times of the nearest points on each ray (not clamped to ray though)
|
||||
// asserts if rays are parallel
|
||||
bool Intersect(const Ray& ra, const Ray& rb, float& ta, float& tb)
|
||||
{
|
||||
Vector3 u = ra.m_start - rb.m_start;
|
||||
|
||||
Scalar a = Dot(ra.m_dir, ra.m_dir);
|
||||
Scalar b = Dot(ra.m_dir, rb.m_dir);
|
||||
Scalar c = Dot(rb.m_dir, rb.m_dir);
|
||||
Scalar d = Dot(ra.m_dir, u);
|
||||
Scalar e = Dot(rb.m_dir, u);
|
||||
|
||||
Scalar det = (a * c - b * b);
|
||||
|
||||
if (Abs(det) < 0.001f)
|
||||
return false;
|
||||
|
||||
Scalar invDet = RcpNr(det);
|
||||
ta = (b * e - c * d) * invDet;
|
||||
tb = (a * e - b * d) * invDet;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Plane
|
||||
bool Plane::IsFinite() const
|
||||
{
|
||||
if (IsNan(GetX()) || IsNan(GetY()) || IsNan(GetZ()) || IsNan(GetW()))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Bounds3 - axis aligned bounding box
|
||||
|
||||
Bounds3::OriginTag Bounds3::Origin;
|
||||
Bounds3::EmptyTag Bounds3::Empty;
|
||||
|
||||
bool Bounds3::Intersect(const Ray& ray, float& tnear, float& tfar) const
|
||||
{
|
||||
Vector3 rcpDir = RcpNr(ray.m_dir);
|
||||
|
||||
Vector3 v1 = (m_min - ray.m_start) * rcpDir;
|
||||
Vector3 v2 = (m_max - ray.m_start) * rcpDir;
|
||||
|
||||
Vector3 vmin = Min(v1, v2);
|
||||
Vector3 vmax = Max(v1, v2);
|
||||
|
||||
Scalar snear = MaxComp(vmin);
|
||||
|
||||
// handle ray being parallel to any axis
|
||||
// (most rays don't need this)
|
||||
if (IsNan(snear))
|
||||
{
|
||||
int inside = (ray.m_start >= m_min) & (ray.m_start <= m_max);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (IsNan(rcpDir.Get(i)))
|
||||
{
|
||||
if ((inside & (1 << i)) == 0)
|
||||
return false;
|
||||
vmin.Set(i, Scalar::Consts::MinValue);
|
||||
vmax.Set(i, Scalar::Consts::MaxValue);
|
||||
}
|
||||
}
|
||||
|
||||
snear = MaxComp(vmin);
|
||||
}
|
||||
|
||||
tnear = snear;
|
||||
tfar = MinComp(vmax);
|
||||
|
||||
if (tnear > tfar)
|
||||
return false;
|
||||
|
||||
if (tfar < 0.0f)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// OrientedBounds3 - oriented bounding box
|
||||
|
||||
#endif
|
||||
#endif //WIN32
|
||||
195
Extras/obsolete/SATConvexCollision/Geometry.h
Normal file
195
Extras/obsolete/SATConvexCollision/Geometry.h
Normal file
@@ -0,0 +1,195 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Geometry.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
#ifndef BULLET_MATH_GEOMETRY_H
|
||||
#define BULLET_MATH_GEOMETRY_H
|
||||
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#include "Vector.h"
|
||||
#include "Matrix.h"
|
||||
|
||||
class Matrix44;
|
||||
class Transform;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Line
|
||||
class Line
|
||||
{
|
||||
public:
|
||||
Point3 m_start;
|
||||
Point3 m_end;
|
||||
|
||||
Line();
|
||||
Line(const Point3& start, const Point3& end);
|
||||
|
||||
// returns false if the lines are parallel
|
||||
friend bool Intersect(const Line& la, const Line& lb, float& ta, float& tb);
|
||||
friend bool IntersectSegments(const Line& la, const Line& lb, float& ta, float& tb);
|
||||
|
||||
// get projection vector between a point and a line
|
||||
// (i.e. if you add the vector to the point, then the new point will lie on the line)
|
||||
friend Vector3 GetProjectionVector(const Line& ln, const Point3& pt);
|
||||
|
||||
// get distance from point to line (and time along line)
|
||||
friend float Distance(const Line& ln, const Point3& pt, float& t);
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Ray
|
||||
class Ray
|
||||
{
|
||||
public:
|
||||
Point3 m_start;
|
||||
Vector3 m_dir;
|
||||
|
||||
Ray();
|
||||
Ray(const Point3& start, const Vector3& dir);
|
||||
|
||||
explicit Ray(const Line& line);
|
||||
|
||||
// returns false if the rays are parallel
|
||||
friend bool Intersect(const Ray& ra, const Ray& rb, float& ta, float& tb);
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Plane
|
||||
class Plane : public Vector4Base
|
||||
{
|
||||
public:
|
||||
// constructors
|
||||
Plane();
|
||||
Plane(const Plane& p);
|
||||
Plane(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w);
|
||||
Plane(const Vector3& xyz, const Scalar& w);
|
||||
Plane(const Point3& a, const Point3& b, const Point3& c);
|
||||
Plane(const Vector3& normal, const Point3& pt);
|
||||
|
||||
// construction to constant
|
||||
Plane(const Maths::ZeroTag&);
|
||||
Plane(const Maths::UnitXTag&);
|
||||
Plane(const Maths::UnitYTag&);
|
||||
Plane(const Maths::UnitZTag&);
|
||||
Plane(const Maths::UnitNegXTag&);
|
||||
Plane(const Maths::UnitNegYTag&);
|
||||
Plane(const Maths::UnitNegZTag&);
|
||||
|
||||
// explicit constructors
|
||||
explicit Plane(const __m128 b);
|
||||
explicit Plane(const Vector3& v);
|
||||
explicit Plane(const Vector4& v);
|
||||
explicit Plane(const float* p);
|
||||
|
||||
// assignment
|
||||
const Plane& operator=(const Plane& v);
|
||||
const Plane& operator=(const Maths::ZeroTag&);
|
||||
const Plane& operator=(const Maths::UnitXTag&);
|
||||
const Plane& operator=(const Maths::UnitYTag&);
|
||||
const Plane& operator=(const Maths::UnitZTag&);
|
||||
const Plane& operator=(const Maths::UnitNegXTag&);
|
||||
const Plane& operator=(const Maths::UnitNegYTag&);
|
||||
const Plane& operator=(const Maths::UnitNegZTag&);
|
||||
|
||||
// element access
|
||||
const Vector3 GetNormal() const;
|
||||
const Scalar getDistance() const;
|
||||
|
||||
// transformations
|
||||
friend const Plane operator-(const Plane& p);
|
||||
friend const Plane operator*(const Plane& p, const Transform& m);
|
||||
|
||||
// operations
|
||||
friend const Scalar Dot(const Plane& p, const Point3& v);
|
||||
friend const Scalar Dot(const Point3& v, const Plane& p);
|
||||
|
||||
friend const Scalar Dot(const Plane& p, const Vector3& v);
|
||||
friend const Scalar Dot(const Vector3& v, const Plane& p);
|
||||
|
||||
friend const Scalar Dot(const Plane& p, const Vector4& v);
|
||||
friend const Scalar Dot(const Vector4& v, const Plane& p);
|
||||
|
||||
friend const Scalar Intersect(const Plane& p, const Ray& ray);
|
||||
friend const Scalar Intersect(const Plane& p, const Line& line);
|
||||
|
||||
// validation
|
||||
bool IsFinite() const;
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Bounds3 - axis aligned bounding box
|
||||
class Bounds3
|
||||
{
|
||||
public:
|
||||
Point3 m_min, m_max;
|
||||
|
||||
static const enum OriginTag { } Origin;
|
||||
static const enum EmptyTag { } Empty;
|
||||
|
||||
// constructors
|
||||
Bounds3();
|
||||
Bounds3(const Bounds3& aabb);
|
||||
Bounds3(const Point3& min, const Point3& max);
|
||||
|
||||
// construction to constant
|
||||
Bounds3(const OriginTag&);
|
||||
Bounds3(const EmptyTag&);
|
||||
|
||||
// explicit constructors
|
||||
explicit Bounds3(const Point3& minMax);
|
||||
|
||||
// assignment
|
||||
const Bounds3& operator=(const Bounds3& aabb);
|
||||
const Bounds3& operator=(const Point3& pt);
|
||||
|
||||
const Bounds3& operator=(const OriginTag&);
|
||||
const Bounds3& operator=(const EmptyTag&);
|
||||
|
||||
// in place operations
|
||||
void operator+=(const Point3& pt);
|
||||
void operator+=(const Bounds3& aabb);
|
||||
|
||||
// operations
|
||||
friend Bounds3 operator+(const Bounds3& aabb, const Point3& pt);
|
||||
friend Bounds3 operator+(const Point3& pt, const Bounds3& aabb);
|
||||
friend Bounds3 operator+(const Bounds3& aabb, const Bounds3& aabb2);
|
||||
|
||||
bool Contains(const Point3& pt) const;
|
||||
bool Contains(const Bounds3& aabb) const;
|
||||
bool Touches(const Bounds3& aabb) const;
|
||||
|
||||
bool Intersect(const Ray& ray, float& tnear, float& tfar) const;
|
||||
bool Intersect(const Line& line, float& tnear, float& tfar) const;
|
||||
|
||||
Point3 GetCenter() const;
|
||||
Vector3 GetExtent() const;
|
||||
Vector3 GetSize() const;
|
||||
|
||||
// validation
|
||||
bool IsFinite() const;
|
||||
bool HasVolume() const;
|
||||
};
|
||||
|
||||
|
||||
#include "Geometry.inl"
|
||||
|
||||
#endif //WIN32
|
||||
#endif //BULLET_MATH_GEOMETRY_H
|
||||
436
Extras/obsolete/SATConvexCollision/Geometry.inl
Normal file
436
Extras/obsolete/SATConvexCollision/Geometry.inl
Normal file
@@ -0,0 +1,436 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Geometry.inl
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#pragma once
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Line
|
||||
|
||||
inline Line::Line()
|
||||
{
|
||||
}
|
||||
|
||||
inline Line::Line(const Point3& start, const Point3& end)
|
||||
{
|
||||
m_start = start;
|
||||
m_end = end;
|
||||
}
|
||||
|
||||
inline Vector3 GetProjectionVector(const Line& ln, const Point3& pt)
|
||||
{
|
||||
Vector3 d = Normalize(ln.m_end - ln.m_start);
|
||||
Vector3 v = pt - ln.m_start;
|
||||
Scalar s = Dot(v, d);
|
||||
return -(v - d * s);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Ray
|
||||
|
||||
inline Ray::Ray()
|
||||
{
|
||||
}
|
||||
|
||||
inline Ray::Ray(const Point3& start, const Vector3& dir)
|
||||
{
|
||||
m_start = start;
|
||||
m_dir = dir;
|
||||
}
|
||||
|
||||
inline Ray::Ray(const Line& line)
|
||||
{
|
||||
m_start = line.m_start;
|
||||
m_dir = Normalize(line.m_end - line.m_start);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Plane
|
||||
|
||||
inline Plane::Plane()
|
||||
{
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Plane& p)
|
||||
{
|
||||
base = p.base;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
|
||||
{
|
||||
Set(x.base, y.base, z.base, w.base);
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Vector3& xyz, const Scalar& w)
|
||||
{
|
||||
Set(xyz.base, w.base);
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Point3& a, const Point3& b, const Point3& c)
|
||||
{
|
||||
Vector3 normal = Normalize(Cross(c - a, b - a));
|
||||
*this = Plane(normal, -Dot(normal, Vector3(a)));
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Vector3& normal, const Point3& pt)
|
||||
{
|
||||
*this = Plane(normal, -Dot(normal, Vector3(pt)));
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::ZeroTag&)
|
||||
{
|
||||
base = _mm_setzero_ps();
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::UnitXTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k1000;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::UnitYTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k0100;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::UnitZTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k0010;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::UnitNegXTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::kNeg1000;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::UnitNegYTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::kNeg0100;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Maths::UnitNegZTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::kNeg0010;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const __m128 b)
|
||||
{
|
||||
base = b;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Vector3& v)
|
||||
{
|
||||
base = v.base;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const Vector4& v)
|
||||
{
|
||||
base = v.base;
|
||||
}
|
||||
|
||||
inline Plane::Plane(const float* p)
|
||||
{
|
||||
base = _mm_load_ps(p);
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Plane& v)
|
||||
{
|
||||
base = v.base;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::ZeroTag&)
|
||||
{
|
||||
base = _mm_setzero_ps();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::UnitXTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k1000;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::UnitYTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k0100;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::UnitZTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k0010;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::UnitNegXTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::kNeg1000;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::UnitNegYTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::kNeg0100;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Plane& Plane::operator=(const Maths::UnitNegZTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::kNeg0010;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Vector3 Plane::GetNormal() const
|
||||
{
|
||||
return Vector3(base);
|
||||
}
|
||||
|
||||
inline const Scalar Plane::getDistance() const
|
||||
{
|
||||
return GetW();
|
||||
}
|
||||
|
||||
inline const Plane operator-(const Plane& p)
|
||||
{
|
||||
return Plane(_mm_sub_ps(_mm_setzero_ps(), p.base));
|
||||
}
|
||||
|
||||
inline const Plane operator*(const Plane& p, const Transform& m)
|
||||
{
|
||||
Vector3 np = Vector3(p) * m;
|
||||
return Plane(np, p.GetW() - Dot(Vector3(np), Vector3(m.GetTranslation())));
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Plane& p, const Point3& v)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot3(p.base, v.base)) + p.GetW();
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Point3& v, const Plane& p)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot3(p.base, v.base)) + p.GetW();
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Plane& p, const Vector3& v)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot3(p.base, v.base));
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Vector3& v, const Plane& p)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot3(p.base, v.base));
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Plane& p, const Vector4& v)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot4(p.base, v.base));
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Vector4& v, const Plane& p)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot4(p.base, v.base));
|
||||
}
|
||||
|
||||
// returns NaN if ray is perpendicular to ray
|
||||
inline const Scalar Intersect(const Plane& p, const Ray& ray)
|
||||
{
|
||||
Scalar ds = Dot(p, ray.m_start);
|
||||
Scalar dd = Dot(p, ray.m_dir);
|
||||
return -ds * RcpNr(dd);
|
||||
}
|
||||
|
||||
// returns NaN if line is perpendicular to ray
|
||||
inline const Scalar Intersect(const Plane& p, const Line& line)
|
||||
{
|
||||
Scalar ds = Dot(p, line.m_start);
|
||||
Scalar de = Dot(p, line.m_end);
|
||||
return ds * RcpNr(ds - de);
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Bounds3 - axis aligned bounding box
|
||||
|
||||
inline Bounds3::Bounds3()
|
||||
{
|
||||
}
|
||||
|
||||
inline Bounds3::Bounds3(const Bounds3& aabb)
|
||||
{
|
||||
*this = aabb;
|
||||
}
|
||||
|
||||
inline Bounds3::Bounds3(const Point3& min, const Point3& max)
|
||||
{
|
||||
m_min = min;
|
||||
m_max = max;
|
||||
}
|
||||
|
||||
inline Bounds3::Bounds3(const OriginTag&)
|
||||
{
|
||||
m_min = m_max = Maths::Zero;
|
||||
}
|
||||
|
||||
inline Bounds3::Bounds3(const EmptyTag&)
|
||||
{
|
||||
// max maximal inverted aabb - ready to have points accumulated into it
|
||||
m_min = Point3(Scalar::Consts::MaxValue);
|
||||
m_max = Point3(Scalar::Consts::MinValue);
|
||||
}
|
||||
|
||||
inline Bounds3::Bounds3(const Point3& minMax)
|
||||
{
|
||||
m_min = m_max = minMax;
|
||||
}
|
||||
|
||||
inline const Bounds3& Bounds3::operator=(const Bounds3& aabb)
|
||||
{
|
||||
m_min = aabb.m_min;
|
||||
m_max = aabb.m_max;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Bounds3& Bounds3::operator=(const Point3& pt)
|
||||
{
|
||||
m_min = m_max = pt;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Bounds3& Bounds3::operator=(const OriginTag&)
|
||||
{
|
||||
m_min = m_max = Maths::Zero;
|
||||
}
|
||||
|
||||
inline const Bounds3& Bounds3::operator=(const EmptyTag&)
|
||||
{
|
||||
// max maximal inverted aabb - ready to have points accumulated into it
|
||||
m_min = Point3(Scalar::Consts::MaxValue);
|
||||
m_max = Point3(Scalar::Consts::MinValue);
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline void Bounds3::operator+=(const Point3& pt)
|
||||
{
|
||||
m_min = Min(m_min, pt);
|
||||
m_max = Max(m_max, pt);
|
||||
}
|
||||
|
||||
inline void Bounds3::operator+=(const Bounds3& aabb)
|
||||
{
|
||||
m_min = Min(m_min, aabb.m_min);
|
||||
m_max = Max(m_max, aabb.m_max);
|
||||
}
|
||||
|
||||
inline Bounds3 operator+(const Bounds3& aabb, const Point3& pt)
|
||||
{
|
||||
return Bounds3(Min(aabb.m_min, pt), Max(aabb.m_max, pt));
|
||||
}
|
||||
|
||||
inline Bounds3 operator+(const Point3& pt, const Bounds3& aabb)
|
||||
{
|
||||
return Bounds3(Min(aabb.m_min, pt), Max(aabb.m_max, pt));
|
||||
}
|
||||
|
||||
inline Bounds3 operator+(const Bounds3& aabb, const Bounds3& aabb2)
|
||||
{
|
||||
return Bounds3(Min(aabb.m_min, aabb2.m_min), Max(aabb.m_max, aabb2.m_max));
|
||||
}
|
||||
|
||||
inline bool Bounds3::Contains(const Point3& pt) const
|
||||
{
|
||||
return ((pt >= m_min) & (pt <= m_max)) == 7;
|
||||
}
|
||||
|
||||
inline bool Bounds3::Contains(const Bounds3& aabb) const
|
||||
{
|
||||
return ((aabb.m_min >= m_min) & (aabb.m_max <= m_max)) == 7;
|
||||
}
|
||||
|
||||
inline bool Bounds3::Touches(const Bounds3& aabb) const
|
||||
{
|
||||
return ((aabb.m_max >= m_min) & (aabb.m_min <= m_max)) == 7;
|
||||
}
|
||||
|
||||
// returns intersection of 2 lines or nearest point to it
|
||||
// t1 and t2 are set to the times of the nearest points on each line
|
||||
// asserts if rays are parallel
|
||||
inline const Point3 IntersectPrev(const Line& la, const Line& lb, float& ta, float& tb)
|
||||
{
|
||||
Vector3 ea = la.m_end - la.m_start;
|
||||
Vector3 eb = lb.m_end - lb.m_start;
|
||||
Vector3 u = la.m_start - lb.m_start;
|
||||
|
||||
Scalar a = Dot(ea, ea);
|
||||
Scalar b = Dot(ea, eb);
|
||||
Scalar c = Dot(eb, eb);
|
||||
Scalar d = Dot(ea, u);
|
||||
Scalar e = Dot(eb, u);
|
||||
|
||||
Scalar det = (a * c - b * b);
|
||||
|
||||
// assert if rays are parallel
|
||||
assert(Abs(det) > Scalar(0.0001f));
|
||||
|
||||
Scalar invDet = RcpNr(det);
|
||||
ta = (b * e - c * d) * invDet;
|
||||
tb = (a * e - b * d) * invDet;
|
||||
|
||||
return la.m_start + ea * ta;
|
||||
}
|
||||
|
||||
inline const Point3 IntersectPrev(const Line& a, const Line& b)
|
||||
{
|
||||
float ta, tb;
|
||||
return IntersectPrev(a, b, ta, tb);
|
||||
}
|
||||
|
||||
inline bool Bounds3::Intersect(const Line& line, float& tnear, float& tfar) const
|
||||
{
|
||||
return Intersect(Ray(line), tnear, tfar);
|
||||
}
|
||||
|
||||
inline Point3 Bounds3::GetCenter() const
|
||||
{
|
||||
return Lerp(m_min, m_max, Scalar::Consts::Half);
|
||||
}
|
||||
|
||||
inline Vector3 Bounds3::GetExtent() const
|
||||
{
|
||||
return (m_max - m_min) * Scalar::Consts::Half;
|
||||
}
|
||||
|
||||
inline Vector3 Bounds3::GetSize() const
|
||||
{
|
||||
return m_max - m_min;
|
||||
}
|
||||
|
||||
inline bool Bounds3::IsFinite() const
|
||||
{
|
||||
return m_min.IsFinite() && m_max.IsFinite();
|
||||
}
|
||||
|
||||
inline bool Bounds3::HasVolume() const
|
||||
{
|
||||
return (((m_min <= m_max) & 7) == 7);
|
||||
}
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// OrientedBounds3 - oriented bounding box
|
||||
|
||||
|
||||
1080
Extras/obsolete/SATConvexCollision/Hull.cpp
Normal file
1080
Extras/obsolete/SATConvexCollision/Hull.cpp
Normal file
File diff suppressed because it is too large
Load Diff
174
Extras/obsolete/SATConvexCollision/Hull.h
Normal file
174
Extras/obsolete/SATConvexCollision/Hull.h
Normal file
@@ -0,0 +1,174 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Hull.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef SAT_HULL_H
|
||||
#define SAT_HULL_H
|
||||
|
||||
#include "Maths.h"
|
||||
#include "Shape.h"
|
||||
|
||||
|
||||
class DynWorld;
|
||||
class HullContactCollector;
|
||||
|
||||
/// Hull implements a convex collision detection algorithm based on Separating Axis Theorem (SAT). It is an alternative to GJK.
|
||||
/// It calculates the separating axis, and based on that it calculates the contact manifold (points) in one go.
|
||||
/// The separating axis calculation is approximated, not all edge-edge calculations are performed (performance reasons).
|
||||
/// Future idea is to combine this with GJK for polyhedra: GJK to calculate the separating axis, and Hull clipping code to calculate the full set of contacts.
|
||||
class Hull : public Shape
|
||||
{
|
||||
friend class ShapeCollider;
|
||||
|
||||
public:
|
||||
struct Edge
|
||||
{
|
||||
short m_verts[2];
|
||||
short m_faces[2];
|
||||
short m_nextEdge[2]; // for each m_face
|
||||
};
|
||||
|
||||
struct Face
|
||||
{
|
||||
short m_numEdges;
|
||||
short m_firstEdge;
|
||||
};
|
||||
|
||||
private:
|
||||
static const int kMaxVerts = 256;
|
||||
static const int kMaxFaces = 256;
|
||||
static const int kMaxEdges = 256;
|
||||
|
||||
short m_numVerts;
|
||||
short m_numFaces;
|
||||
short m_numEdges;
|
||||
|
||||
Point3* m_pVerts;
|
||||
Face* m_pFaces;
|
||||
Edge* m_pEdges;
|
||||
Plane* m_pPlanes;
|
||||
|
||||
// hull construction stuff
|
||||
static const int kTmpFaceMaxVerts = 64;
|
||||
struct TmpFace
|
||||
{
|
||||
short m_index;
|
||||
short m_next;
|
||||
short m_numVerts;
|
||||
short m_verts[kTmpFaceMaxVerts];
|
||||
short m_edges[kTmpFaceMaxVerts];
|
||||
Plane m_plane;
|
||||
};
|
||||
|
||||
struct TmpEdge
|
||||
{
|
||||
short m_index;
|
||||
short m_next;
|
||||
short m_verts[2];
|
||||
short m_faces[2];
|
||||
};
|
||||
|
||||
static short s_firstFreeTmpFace;
|
||||
static short s_firstUsedTmpFace;
|
||||
static TmpFace* s_pTmpFaces;
|
||||
|
||||
static short s_firstFreeTmpEdge;
|
||||
static short s_firstUsedTmpEdge;
|
||||
static TmpEdge* s_pTmpEdges;
|
||||
|
||||
static const Point3* s_pPoints;
|
||||
|
||||
static short AllocTmpFace();
|
||||
static void FreeTmpFace(short face);
|
||||
static TmpFace* GetTmpFace(short index) {if (index < 0) return 0; return s_pTmpFaces + index;}
|
||||
|
||||
static short AllocTmpEdge();
|
||||
static void FreeTmpEdge(short edge);
|
||||
static TmpEdge* GetTmpEdge(short index) {if (index < 0) return 0; return s_pTmpEdges + index;}
|
||||
|
||||
static short MatchOrAddEdge(short vert0, short vert1, short face);
|
||||
static void UnmatchOrRemoveEdge(short edge, short face);
|
||||
|
||||
static short AddTmpFace(short vert0, short vert1, short vert2);
|
||||
static short AddTmpFace(short numVerts, short* pVerts);
|
||||
static short AddTmpFace(short vert0, short numOtherVerts, short* pVerts);
|
||||
static void RemoveTmpFace(short face);
|
||||
|
||||
static bool TmpFaceAddPoint(short point, short face);
|
||||
|
||||
static int RemoveVisibleFaces(const Point3& point);
|
||||
static void FillHole(short newVertex);
|
||||
static Hull* MakeHullFromTemp();
|
||||
|
||||
public:
|
||||
Hull();
|
||||
~Hull();
|
||||
|
||||
// ObjectType GetObjectType() const {return kTypeHull;}
|
||||
|
||||
short getNumVertices() const;
|
||||
short GetNumFaces() const;
|
||||
short getNumEdges() const;
|
||||
|
||||
const Point3& getVertex(short index) const;
|
||||
const Face& GetFace(short index) const;
|
||||
const Edge& getEdge(short index) const;
|
||||
const Plane& getPlane(short index) const;
|
||||
|
||||
short GetFaceFirstEdge(short face) const;
|
||||
short GetFaceNextEdge(short face, short prevEdge) const;
|
||||
|
||||
short GetEdgeVertex0(short face, short edge) const;
|
||||
short GetEdgeVertex1(short face, short edge) const;
|
||||
|
||||
short GetEdgeOtherFace(short edge, short face) const;
|
||||
|
||||
Point3 GetFaceCentroid(short face) const;
|
||||
|
||||
//static void ProcessHullHull(btSeparation& sep);
|
||||
static void ProcessHullHull(btSeparation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector);
|
||||
|
||||
virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const;
|
||||
virtual Bounds3 ComputeBounds(const Transform& transform) const;
|
||||
|
||||
static Hull* MakeHull(int numPoints, const Point3* pPoints);
|
||||
|
||||
//for contact generation
|
||||
|
||||
|
||||
|
||||
/// Clips a face to the back of a plane
|
||||
static int ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane);
|
||||
|
||||
static bool GetSeparationHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
|
||||
const Transform& trA, const Transform& trB,
|
||||
const Hull& hullA,
|
||||
const Hull& hullB
|
||||
);
|
||||
|
||||
static int AddContactsHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
|
||||
const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB,
|
||||
HullContactCollector* hullContactCollector);
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#include "hull.inl"
|
||||
|
||||
#endif //SAT_HULL_H
|
||||
100
Extras/obsolete/SATConvexCollision/Hull.inl
Normal file
100
Extras/obsolete/SATConvexCollision/Hull.inl
Normal file
@@ -0,0 +1,100 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Hull.inl
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#pragma once
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
inline short Hull::getNumVertices() const
|
||||
{
|
||||
return m_numVerts;
|
||||
}
|
||||
|
||||
inline short Hull::GetNumFaces() const
|
||||
{
|
||||
return m_numFaces;
|
||||
}
|
||||
|
||||
inline short Hull::getNumEdges() const
|
||||
{
|
||||
return m_numEdges;
|
||||
}
|
||||
|
||||
inline const Point3& Hull::getVertex(short index) const
|
||||
{
|
||||
return m_pVerts[index];
|
||||
}
|
||||
|
||||
inline const Hull::Face& Hull::GetFace(short index) const
|
||||
{
|
||||
return m_pFaces[index];
|
||||
}
|
||||
|
||||
inline const Hull::Edge& Hull::GetEdge(short index) const
|
||||
{
|
||||
return m_pEdges[index];
|
||||
}
|
||||
|
||||
inline const Plane& Hull::GetPlane(short index) const
|
||||
{
|
||||
return m_pPlanes[index];
|
||||
}
|
||||
|
||||
inline short Hull::GetFaceFirstEdge(short face) const
|
||||
{
|
||||
assert(face >= 0 && face < m_numFaces);
|
||||
|
||||
return m_pFaces[face].m_firstEdge;
|
||||
}
|
||||
|
||||
inline short Hull::GetFaceNextEdge(short face, short prevEdge) const
|
||||
{
|
||||
assert(face >= 0 && face < m_numFaces);
|
||||
assert(prevEdge >= 0 && prevEdge < m_numEdges);
|
||||
|
||||
const Edge& e = m_pEdges[prevEdge];
|
||||
return e.m_nextEdge[face == e.m_faces[1]];
|
||||
}
|
||||
|
||||
inline short Hull::GetEdgeVertex0(short face, short edge) const
|
||||
{
|
||||
assert(face >= 0 && face < m_numFaces);
|
||||
assert(edge >= 0 && edge < m_numEdges);
|
||||
|
||||
const Edge& e = m_pEdges[edge];
|
||||
return e.m_verts[face == e.m_faces[0]];
|
||||
}
|
||||
|
||||
inline short Hull::GetEdgeVertex1(short face, short edge) const
|
||||
{
|
||||
assert(face >= 0 && face < m_numFaces);
|
||||
assert(edge >= 0 && edge < m_numEdges);
|
||||
|
||||
const Edge& e = m_pEdges[edge];
|
||||
return e.m_verts[face == e.m_faces[1]];
|
||||
}
|
||||
|
||||
inline short Hull::GetEdgeOtherFace(short edge, short face) const
|
||||
{
|
||||
assert(face >= 0 && face < m_numFaces);
|
||||
assert(edge >= 0 && edge < m_numEdges);
|
||||
|
||||
const Edge& e = m_pEdges[edge];
|
||||
assert(e.m_faces[0] == face || e.m_faces[1] == face);
|
||||
|
||||
return e.m_faces[face == e.m_faces[0]];
|
||||
}
|
||||
38
Extras/obsolete/SATConvexCollision/HullContactCollector.h
Normal file
38
Extras/obsolete/SATConvexCollision/HullContactCollector.h
Normal file
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef HULL_CONTACT_COLLECTOR_H
|
||||
#define HULL_CONTACT_COLLECTOR_H
|
||||
|
||||
class Vector3;
|
||||
class Point3;
|
||||
class Scalar;
|
||||
struct btSeparation;
|
||||
|
||||
///HullContactCollector collects the Hull computation to the contact point results
|
||||
class HullContactCollector
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~HullContactCollector() {};
|
||||
|
||||
virtual int BatchAddContactGroup(const btSeparation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0;
|
||||
|
||||
virtual int GetMaxNumContacts() const = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif //HULL_CONTACT_COLLECTOR_H
|
||||
175
Extras/obsolete/SATConvexCollision/Maths.h
Normal file
175
Extras/obsolete/SATConvexCollision/Maths.h
Normal file
@@ -0,0 +1,175 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Maths.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef BULLET_MATH_H
|
||||
#define BULLET_MATH_H
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <float.h>
|
||||
|
||||
// intrinsics headers
|
||||
#include <mmintrin.h>
|
||||
#include <xmmintrin.h>
|
||||
|
||||
// vector maths classes require aligned alloc
|
||||
#include "Memory2.h"
|
||||
|
||||
// constants
|
||||
#define PI 3.141592654f
|
||||
|
||||
#define Angle5 0.087266462f
|
||||
#define Angle10 0.174532925f
|
||||
#define Angle15 0.261799388f
|
||||
#define Angle30 0.523598776f
|
||||
#define Angle45 0.785398163f
|
||||
#define Angle60 0.523598776f
|
||||
#define Angle90 1.570796327f
|
||||
#define Angle180 PI
|
||||
#define Angle270 4.71238898f
|
||||
#define Angle360 6.283185307f
|
||||
|
||||
#define Deg2RadF 0.01745329251994329547f
|
||||
#define Rad2DegF 57.29577951308232286465f
|
||||
|
||||
#define MinValueF -3.402823466e+38f
|
||||
#define MaxValueF 3.402823466e+38F
|
||||
|
||||
#define DefaultEpsilon 0.001f
|
||||
|
||||
|
||||
// float functions
|
||||
|
||||
inline float Sin(const float f)
|
||||
{
|
||||
return sinf(f);
|
||||
}
|
||||
|
||||
inline float Cos(const float f)
|
||||
{
|
||||
return cosf(f);
|
||||
}
|
||||
|
||||
inline float Tan(const float f)
|
||||
{
|
||||
return tanf(f);
|
||||
}
|
||||
|
||||
inline float Asin(const float f)
|
||||
{
|
||||
return asinf(f);
|
||||
}
|
||||
|
||||
inline float Acos(const float f)
|
||||
{
|
||||
return acosf(f);
|
||||
}
|
||||
|
||||
inline float Atan(const float f)
|
||||
{
|
||||
return atanf(f);
|
||||
}
|
||||
|
||||
inline float Atan2(const float y, const float x)
|
||||
{
|
||||
return atan2f(y, x);
|
||||
}
|
||||
|
||||
inline float Pow(const float x, const float y)
|
||||
{
|
||||
return powf(x, y);
|
||||
}
|
||||
|
||||
inline float Abs(const float x)
|
||||
{
|
||||
return fabsf(x);
|
||||
}
|
||||
|
||||
inline float Min(const float x, const float y)
|
||||
{
|
||||
return (x < y) ? x : y;
|
||||
}
|
||||
|
||||
inline float Max(const float x, const float y)
|
||||
{
|
||||
return (x > y) ? x : y;
|
||||
}
|
||||
|
||||
inline float Clamp(const float x, const float min, const float max)
|
||||
{
|
||||
return (x >= max) ? max : Max(x, min);
|
||||
}
|
||||
|
||||
inline float Sgn(const float f)
|
||||
{
|
||||
// TODO: replace this with something that doesn't branch
|
||||
if (f < 0.0f)
|
||||
return -1.0f;
|
||||
if (f > 0.0f)
|
||||
return 1.0f;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
inline float Floor(const float f)
|
||||
{
|
||||
return floorf(f);
|
||||
}
|
||||
|
||||
inline float Ceil(const float f)
|
||||
{
|
||||
return ceilf(f);
|
||||
}
|
||||
|
||||
inline float Mod(const float x, const float y)
|
||||
{
|
||||
float n = Floor(x / y);
|
||||
return x - n * y;
|
||||
}
|
||||
|
||||
inline float Sqrt(const float f)
|
||||
{
|
||||
return sqrtf(f);
|
||||
}
|
||||
|
||||
inline bool Equal(const float x, const float y, const float epsilon = DefaultEpsilon)
|
||||
{
|
||||
return Abs(x-y) <= epsilon;
|
||||
}
|
||||
|
||||
inline float Lerp(const float x, const float y, const float t)
|
||||
{
|
||||
return x + (y - x) * t;
|
||||
}
|
||||
|
||||
inline int Min(const int x, const int y)
|
||||
{
|
||||
return (x < y) ? x : y;
|
||||
}
|
||||
|
||||
inline int Max(const int x, const int y)
|
||||
{
|
||||
return (x > y) ? x : y;
|
||||
}
|
||||
|
||||
#include "Vector.h"
|
||||
#include "Matrix.h"
|
||||
#include "Quat.h"
|
||||
#include "Geometry.h"
|
||||
|
||||
#endif //WIN32
|
||||
#endif //BULLET_MATH_H
|
||||
204
Extras/obsolete/SATConvexCollision/Matrix.cpp
Normal file
204
Extras/obsolete/SATConvexCollision/Matrix.cpp
Normal file
@@ -0,0 +1,204 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Matrix.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
|
||||
#include "matrix.h"
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Matrix33
|
||||
Matrix33::Matrix33(const Quat& q)
|
||||
{
|
||||
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
||||
float *pIn = (float*)&q;
|
||||
float *pOut = (float*)this;
|
||||
|
||||
float x = pIn[0], y = pIn[1], z = pIn[2], w = pIn[3];
|
||||
|
||||
x2 = x + x; y2 = y + y; z2 = z + z;
|
||||
xx = x * x2; yy = y * y2; zz = z * z2;
|
||||
wx = w * x2; wy = w * y2; wz = w * z2;
|
||||
xy = x * y2; xz = x * z2;
|
||||
yz = y * z2;
|
||||
|
||||
pOut[0] = 1.0f - (yy + zz);
|
||||
pOut[1] = xy + wz;
|
||||
pOut[2] = xz - wy;
|
||||
pOut[3] = 0.0f;
|
||||
|
||||
pOut[4] = xy - wz;
|
||||
pOut[5] = 1.0f - (xx + zz);
|
||||
pOut[6] = yz + wx;
|
||||
pOut[7] = 0.0f;
|
||||
|
||||
pOut[8] = xz + wy;
|
||||
pOut[9] = yz - wx;
|
||||
pOut[10] = 1.0f - (xx + yy);
|
||||
pOut[11] = 0.0f;
|
||||
}
|
||||
|
||||
const Matrix33 Inv(const Matrix33& m)
|
||||
{
|
||||
// TODO: do this efficiently - for now we just use the Matrix44 version
|
||||
Matrix44 m44 = Inv(Matrix44(Vector4(m[0]), Vector4(m[1]), Vector4(m[2]), Vector4(Maths::UnitW)));
|
||||
return Matrix33(Vector3(m44[0]), Vector3(m44[1]), Vector3(m44[2]));
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Matrix44
|
||||
|
||||
// calculate the inverse of a general 4x4 matrix
|
||||
//
|
||||
// -1
|
||||
// A = ___1__ adjoint A
|
||||
// det A
|
||||
//
|
||||
const Matrix44 Inv(const Matrix44& src)
|
||||
{
|
||||
__m128 minor0, minor1, minor2, minor3;
|
||||
__m128 row0, row1 = _mm_set_ps1(0.0f), row2, row3 = row1;
|
||||
__m128 det, tmp1 = row1, tmp2;
|
||||
|
||||
Matrix44 tmp(src);
|
||||
float *m = (float*)&tmp;
|
||||
|
||||
tmp1 = _mm_loadh_pi( _mm_loadl_pi( tmp1, (__m64*)(m ) ), (__m64*)(m+ 4) );
|
||||
row1 = _mm_loadh_pi( _mm_loadl_pi( row1, (__m64*)(m+8) ), (__m64*)(m+12) );
|
||||
|
||||
row0 = _mm_shuffle_ps( tmp1, row1, 0x88 );
|
||||
row1 = _mm_shuffle_ps( row1, tmp1, 0xDD );
|
||||
|
||||
tmp1 = _mm_loadh_pi( _mm_loadl_pi( tmp1, (__m64*)(m+ 2) ), (__m64*)(m+ 6) );
|
||||
row3 = _mm_loadh_pi( _mm_loadl_pi( row3, (__m64*)(m+10) ), (__m64*)(m+14) );
|
||||
|
||||
row2 = _mm_shuffle_ps( tmp1, row3, 0x88 );
|
||||
row3 = _mm_shuffle_ps( row3, tmp1, 0xDD );
|
||||
// -----------------------------------------------
|
||||
tmp2 = _mm_mul_ps( row2, row3 );
|
||||
tmp1 = _mm_shuffle_ps( tmp2, tmp2, 0xB1);
|
||||
|
||||
minor0 = _mm_mul_ps( row1, tmp1 );
|
||||
minor1 = _mm_mul_ps( row0, tmp1 );
|
||||
|
||||
tmp1 = _mm_shuffle_ps( tmp1, tmp1, 0x4E );
|
||||
|
||||
minor0 = _mm_sub_ps( _mm_mul_ps( row1, tmp1 ), minor0 );
|
||||
minor1 = _mm_sub_ps( _mm_mul_ps( row0, tmp1 ), minor1 );
|
||||
minor1 = _mm_shuffle_ps( minor1, minor1, 0x4E );
|
||||
// -----------------------------------------------
|
||||
tmp2 = _mm_mul_ps( row1, row2);
|
||||
tmp1 = _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
|
||||
|
||||
minor0 = _mm_add_ps( _mm_mul_ps( row3, tmp1 ), minor0 );
|
||||
minor3 = _mm_mul_ps( row0, tmp1 );
|
||||
|
||||
tmp1 = _mm_shuffle_ps( tmp1, tmp1, 0x4E );
|
||||
|
||||
minor0 = _mm_sub_ps( minor0, _mm_mul_ps( row3, tmp1 ) );
|
||||
minor3 = _mm_sub_ps( _mm_mul_ps( row0, tmp1 ), minor3 );
|
||||
minor3 = _mm_shuffle_ps( minor3, minor3, 0x4E );
|
||||
// -----------------------------------------------
|
||||
tmp2 = _mm_mul_ps( _mm_shuffle_ps( row1, row1, 0x4E ), row3 );
|
||||
tmp1 = _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
|
||||
row2 = _mm_shuffle_ps( row2, row2, 0x4E );
|
||||
|
||||
minor0 = _mm_add_ps( _mm_mul_ps( row2, tmp1 ), minor0 );
|
||||
minor2 = _mm_mul_ps( row0, tmp1 );
|
||||
|
||||
tmp1 = _mm_shuffle_ps( tmp1, tmp1, 0x4E );
|
||||
|
||||
minor0 = _mm_sub_ps( minor0, _mm_mul_ps( row2, tmp1 ) );
|
||||
minor2 = _mm_sub_ps( _mm_mul_ps( row0, tmp1 ), minor2 );
|
||||
minor2 = _mm_shuffle_ps( minor2, minor2, 0x4E );
|
||||
// -----------------------------------------------
|
||||
tmp2 = _mm_mul_ps( row0, row1);
|
||||
tmp1 = _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
|
||||
|
||||
minor2 = _mm_add_ps( _mm_mul_ps( row3, tmp1 ), minor2 );
|
||||
minor3 = _mm_sub_ps( _mm_mul_ps( row2, tmp1 ), minor3 );
|
||||
|
||||
tmp1 = _mm_shuffle_ps( tmp1, tmp1, 0x4E );
|
||||
|
||||
minor2 = _mm_sub_ps( _mm_mul_ps( row3, tmp1 ), minor2 );
|
||||
minor3 = _mm_sub_ps( minor3, _mm_mul_ps( row2, tmp1 ) );
|
||||
// -----------------------------------------------
|
||||
tmp2 = _mm_mul_ps( row0, row3);
|
||||
tmp1 = _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
|
||||
|
||||
minor1 = _mm_sub_ps( minor1, _mm_mul_ps( row2, tmp1 ) );
|
||||
minor2 = _mm_add_ps( _mm_mul_ps( row1, tmp1 ), minor2 );
|
||||
|
||||
tmp1 = _mm_shuffle_ps( tmp1, tmp1, 0x4E );
|
||||
|
||||
minor1 = _mm_add_ps( _mm_mul_ps( row2, tmp1 ), minor1 );
|
||||
minor2 = _mm_sub_ps( minor2, _mm_mul_ps( row1, tmp1 ) );
|
||||
// -----------------------------------------------
|
||||
tmp2 = _mm_mul_ps( row0, row2);
|
||||
tmp1 = _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
|
||||
|
||||
minor1 = _mm_add_ps( _mm_mul_ps( row3, tmp1 ), minor1 );
|
||||
minor3 = _mm_sub_ps( minor3, _mm_mul_ps( row1, tmp1 ) );
|
||||
|
||||
tmp1 = _mm_shuffle_ps( tmp1, tmp1, 0x4E );
|
||||
|
||||
minor1 = _mm_sub_ps( minor1, _mm_mul_ps( row3, tmp1 ) );
|
||||
minor3 = _mm_add_ps( _mm_mul_ps( row1, tmp1 ), minor3 );
|
||||
// -----------------------------------------------
|
||||
det = _mm_mul_ps( row0, minor0 );
|
||||
det = _mm_add_ps( _mm_shuffle_ps( det, det, 0x4E ), det );
|
||||
det = _mm_add_ss( _mm_shuffle_ps( det, det, 0xB1 ), det );
|
||||
tmp1 = _mm_rcp_ss( det );
|
||||
|
||||
det = _mm_sub_ss( _mm_add_ss( tmp1, tmp1 ), _mm_mul_ss( det, _mm_mul_ss( tmp1, tmp1 ) ) );
|
||||
det = _mm_shuffle_ps( det, det, 0x00 );
|
||||
|
||||
minor0 = _mm_mul_ps( det, minor0 );
|
||||
_mm_storel_pi( (__m64*)( m ), minor0 );
|
||||
_mm_storeh_pi( (__m64*)(m+2), minor0 );
|
||||
|
||||
minor1 = _mm_mul_ps( det, minor1 );
|
||||
_mm_storel_pi( (__m64*)(m+4), minor1 );
|
||||
_mm_storeh_pi( (__m64*)(m+6), minor1 );
|
||||
|
||||
minor2 = _mm_mul_ps( det, minor2 );
|
||||
_mm_storel_pi( (__m64*)(m+ 8), minor2 );
|
||||
_mm_storeh_pi( (__m64*)(m+10), minor2 );
|
||||
|
||||
minor3 = _mm_mul_ps( det, minor3 );
|
||||
_mm_storel_pi( (__m64*)(m+12), minor3 );
|
||||
_mm_storeh_pi( (__m64*)(m+14), minor3 );
|
||||
|
||||
return tmp;
|
||||
}
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Transform
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Matrix66
|
||||
|
||||
#endif
|
||||
#endif //#ifdef WIN32
|
||||
|
||||
208
Extras/obsolete/SATConvexCollision/Matrix.h
Normal file
208
Extras/obsolete/SATConvexCollision/Matrix.h
Normal file
@@ -0,0 +1,208 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Matrix.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef BULLET_MATRIX_H
|
||||
#define BULLET_MATRIX_H
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#include "Vector.h"
|
||||
#include "Memory2.h"
|
||||
|
||||
class Quat;
|
||||
|
||||
#include <malloc.h>
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
/// Matrix33
|
||||
__declspec(align(16)) class Matrix33
|
||||
{
|
||||
private:
|
||||
Vector3 m_rows[3];
|
||||
|
||||
public:
|
||||
|
||||
BULLET_ALIGNED_NEW_AND_DELETE
|
||||
|
||||
|
||||
// constructors
|
||||
Matrix33();
|
||||
Matrix33(const Vector3& x, const Vector3& y, const Vector3& z);
|
||||
|
||||
// explicit constructors
|
||||
explicit Matrix33(const Quat& q);
|
||||
|
||||
explicit Matrix33(const Maths::ZeroTag&);
|
||||
explicit Matrix33(const Maths::IdentityTag&);
|
||||
explicit Matrix33(const Maths::RotateXTag&, float radians);
|
||||
explicit Matrix33(const Maths::RotateYTag&, float radians);
|
||||
explicit Matrix33(const Maths::RotateZTag&, float radians);
|
||||
explicit Matrix33(const Maths::ScaleTag&, const Vector3& scale);
|
||||
explicit Matrix33(const Maths::SkewTag&, const Vector3& v);
|
||||
|
||||
// assignment
|
||||
const Matrix33& operator=(const Matrix33& m);
|
||||
|
||||
// assignment to constant
|
||||
const Matrix33& operator=(const Maths::ZeroTag&);
|
||||
const Matrix33& operator=(const Maths::IdentityTag&);
|
||||
|
||||
// element access
|
||||
Vector3& operator[](int row);
|
||||
const Vector3& operator[](int row) const;
|
||||
|
||||
Vector3& GetAxisX();
|
||||
const Vector3& GetAxisX() const;
|
||||
void SetAxisX(const Vector3& v);
|
||||
|
||||
Vector3& GetAxisY();
|
||||
const Vector3& GetAxisY() const;
|
||||
void SetAxisY(const Vector3& v);
|
||||
|
||||
Vector3& GetAxisZ();
|
||||
const Vector3& GetAxisZ() const;
|
||||
void SetAxisZ(const Vector3& v);
|
||||
|
||||
// operations
|
||||
void operator*=(const Matrix33& a);
|
||||
void operator*=(const Scalar& s);
|
||||
void operator+=(const Matrix33& a);
|
||||
void operator-=(const Matrix33& a);
|
||||
|
||||
friend const Vector3 operator*(const Vector3& v, const Matrix33& m);
|
||||
friend const Vector3 operator*(const Matrix33& m, const Vector3& vT);
|
||||
friend const Matrix33 operator*(const Matrix33& a, const Matrix33& b);
|
||||
friend const Matrix33 operator*(const Matrix33& m, const Scalar& s);
|
||||
friend const Matrix33 operator+(const Matrix33& a, const Matrix33& b);
|
||||
friend const Matrix33 operator-(const Matrix33& a, const Matrix33& b);
|
||||
|
||||
friend const Matrix33 Transpose(const Matrix33& m);
|
||||
friend const Matrix33 Inv(const Matrix33& m);
|
||||
friend const Scalar Det(const Matrix33& m);
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Matrix44
|
||||
|
||||
__declspec(align(16)) class Matrix44
|
||||
{
|
||||
private:
|
||||
Vector4 m_rows[4];
|
||||
|
||||
public:
|
||||
// constructors
|
||||
Matrix44();
|
||||
Matrix44(const Vector4& x, const Vector4& y, const Vector4& z, const Vector4& w);
|
||||
|
||||
// explicit constructors
|
||||
explicit Matrix44(const Maths::ZeroTag&);
|
||||
explicit Matrix44(const Maths::IdentityTag&);
|
||||
explicit Matrix44(const Maths::RotateXTag&, float radians);
|
||||
explicit Matrix44(const Maths::RotateYTag&, float radians);
|
||||
explicit Matrix44(const Maths::RotateZTag&, float radians);
|
||||
|
||||
// assignment
|
||||
const Matrix44& operator=(const Matrix44& m);
|
||||
|
||||
// assignment to constant
|
||||
const Matrix44& operator=(const Maths::ZeroTag&);
|
||||
const Matrix44& operator=(const Maths::IdentityTag&);
|
||||
|
||||
// element access
|
||||
Vector4& operator[](int row);
|
||||
const Vector4& operator[](int row) const;
|
||||
|
||||
// operations
|
||||
void operator*=(const Matrix44& a);
|
||||
void operator*=(const Scalar& s);
|
||||
void operator+=(const Matrix44& a);
|
||||
void operator-=(const Matrix44& a);
|
||||
|
||||
friend const Vector3 operator*(const Vector3& v, const Matrix44& m);
|
||||
friend const Point3 operator*(const Point3& v, const Matrix44& m);
|
||||
friend const Vector4 operator*(const Vector4& v, const Matrix44& m);
|
||||
|
||||
friend const Matrix44 operator*(const Matrix44& a, const Matrix44& b);
|
||||
friend const Matrix44 operator*(const Scalar& s, const Matrix44& m);
|
||||
friend const Matrix44 operator*(const Matrix44& m, const Scalar& s);
|
||||
friend const Matrix44 operator+(const Matrix44& a, const Matrix44& b);
|
||||
friend const Matrix44 operator-(const Matrix44& a, const Matrix44& b);
|
||||
|
||||
friend const Matrix44 Transpose(const Matrix44& m);
|
||||
friend const Matrix44 Inv(const Matrix44& m);
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Transform
|
||||
|
||||
__declspec(align(16)) class Transform
|
||||
{
|
||||
private:
|
||||
Matrix33 m_rotation;
|
||||
Point3 m_translation;
|
||||
|
||||
public:
|
||||
// constructors
|
||||
Transform();
|
||||
Transform(const Matrix33& xyz, const Point3& w);
|
||||
Transform(const Vector3& x, const Vector3& y, const Vector3& z, const Point3& w);
|
||||
|
||||
// explicit constructors
|
||||
explicit Transform(const Maths::IdentityTag&);
|
||||
|
||||
// assignment
|
||||
const Transform& operator=(const Transform& m);
|
||||
|
||||
// assignment to constant
|
||||
const Transform& operator=(const Maths::IdentityTag&);
|
||||
|
||||
// element access
|
||||
Matrix33& GetRotation();
|
||||
const Matrix33& GetRotation() const;
|
||||
void SetRotation(const Matrix33& m);
|
||||
void SetRotation(const Quat& q);
|
||||
|
||||
Point3& GetTranslation();
|
||||
const Point3& GetTranslation() const;
|
||||
void SetTranslation(const Point3& p);
|
||||
|
||||
Vector3& GetAxisX();
|
||||
const Vector3& GetAxisX() const;
|
||||
|
||||
Vector3& GetAxisY();
|
||||
const Vector3& GetAxisY() const;
|
||||
|
||||
Vector3& GetAxisZ();
|
||||
const Vector3& GetAxisZ() const;
|
||||
|
||||
// operations
|
||||
friend const Vector3 operator*(const Vector3& v, const Transform& m);
|
||||
friend const Point3 operator*(const Point3& v, const Transform& m);
|
||||
|
||||
friend const Transform operator*(const Transform& v, const Transform& m);
|
||||
|
||||
friend const Transform Inv(const Transform& m);
|
||||
};
|
||||
|
||||
|
||||
|
||||
#include "matrix.inl"
|
||||
|
||||
#endif //#ifdef WIN32
|
||||
#endif //BULLET_MATRIX_H
|
||||
609
Extras/obsolete/SATConvexCollision/Matrix.inl
Normal file
609
Extras/obsolete/SATConvexCollision/Matrix.inl
Normal file
@@ -0,0 +1,609 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Matrix.inl
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#pragma once
|
||||
|
||||
#include <assert.h>
|
||||
#include "math.h"
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Matrix33
|
||||
|
||||
inline Matrix33::Matrix33()
|
||||
{
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Vector3& x, const Vector3& y, const Vector3& z)
|
||||
{
|
||||
m_rows[0] = x;
|
||||
m_rows[1] = y;
|
||||
m_rows[2] = z;
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::ZeroTag&)
|
||||
{
|
||||
m_rows[0] = Maths::Zero;
|
||||
m_rows[1] = Maths::Zero;
|
||||
m_rows[2] = Maths::Zero;
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::IdentityTag&)
|
||||
{
|
||||
m_rows[0] = Maths::UnitX;
|
||||
m_rows[1] = Maths::UnitY;
|
||||
m_rows[2] = Maths::UnitZ;
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::RotateXTag&, float radians)
|
||||
{
|
||||
float c = cosf(radians);
|
||||
float s = sinf(radians);
|
||||
|
||||
m_rows[0] = Maths::UnitX;
|
||||
m_rows[1] = Vector3(0.0f, c, s);
|
||||
m_rows[2] = Vector3(0.0f, -s, c);
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::RotateYTag&, float radians)
|
||||
{
|
||||
float c = cosf(radians);
|
||||
float s = sinf(radians);
|
||||
|
||||
m_rows[0] = Vector3(c, 0.0f, -s);
|
||||
m_rows[1] = Maths::UnitY;
|
||||
m_rows[2] = Vector3(s, 0.0f, c);
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::RotateZTag&, float radians)
|
||||
{
|
||||
float c = cosf(radians);
|
||||
float s = sinf(radians);
|
||||
|
||||
m_rows[0] = Vector3(c, s, 0.0f);
|
||||
m_rows[1] = Vector3(-s, c, 0.0f);
|
||||
m_rows[2] = Maths::UnitZ;
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::ScaleTag&, const Vector3& scale)
|
||||
{
|
||||
m_rows[0] = scale * Vector3(Maths::UnitX);
|
||||
m_rows[1] = scale * Vector3(Maths::UnitY);
|
||||
m_rows[2] = scale * Vector3(Maths::UnitZ);
|
||||
}
|
||||
|
||||
inline Matrix33::Matrix33(const Maths::SkewTag&, const Vector3& v)
|
||||
{
|
||||
m_rows[0] = Vector3(0.0f, v[2], -v[1]);
|
||||
m_rows[1] = Vector3(-v[2], 0.0f, v[0]);
|
||||
m_rows[2] = Vector3(v[1], -v[0], 0.0f);
|
||||
}
|
||||
|
||||
inline const Matrix33& Matrix33::operator=(const Matrix33& m)
|
||||
{
|
||||
m_rows[0] = m.m_rows[0];
|
||||
m_rows[1] = m.m_rows[1];
|
||||
m_rows[2] = m.m_rows[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Matrix33& Matrix33::operator=(const Maths::ZeroTag&)
|
||||
{
|
||||
m_rows[0] = m_rows[1] = m_rows[2] = Maths::Zero;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Matrix33& Matrix33::operator=(const Maths::IdentityTag&)
|
||||
{
|
||||
m_rows[0] = Maths::UnitX;
|
||||
m_rows[1] = Maths::UnitY;
|
||||
m_rows[2] = Maths::UnitZ;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline Vector3& Matrix33::operator[](int row)
|
||||
{
|
||||
return m_rows[row];
|
||||
}
|
||||
|
||||
inline const Vector3& Matrix33::operator[](int row) const
|
||||
{
|
||||
return m_rows[row];
|
||||
}
|
||||
|
||||
inline Vector3& Matrix33::GetAxisX()
|
||||
{
|
||||
return m_rows[0];
|
||||
}
|
||||
|
||||
inline const Vector3& Matrix33::GetAxisX() const
|
||||
{
|
||||
return m_rows[0];
|
||||
}
|
||||
|
||||
inline void Matrix33::SetAxisX(const Vector3& v)
|
||||
{
|
||||
m_rows[0] = v;
|
||||
}
|
||||
|
||||
inline Vector3& Matrix33::GetAxisY()
|
||||
{
|
||||
return m_rows[1];
|
||||
}
|
||||
|
||||
inline const Vector3& Matrix33::GetAxisY() const
|
||||
{
|
||||
return m_rows[1];
|
||||
}
|
||||
|
||||
inline void Matrix33::SetAxisY(const Vector3& v)
|
||||
{
|
||||
m_rows[1] = v;
|
||||
}
|
||||
|
||||
inline Vector3& Matrix33::GetAxisZ()
|
||||
{
|
||||
return m_rows[2];
|
||||
}
|
||||
|
||||
inline const Vector3& Matrix33::GetAxisZ() const
|
||||
{
|
||||
return m_rows[2];
|
||||
}
|
||||
|
||||
inline void Matrix33::SetAxisZ(const Vector3& v)
|
||||
{
|
||||
m_rows[2] = v;
|
||||
}
|
||||
|
||||
inline const Vector3 operator*(const Vector3& v, const Matrix33& m)
|
||||
{
|
||||
Scalar xxxx = v.GetX();
|
||||
Scalar yyyy = v.GetY();
|
||||
Scalar zzzz = v.GetZ();
|
||||
|
||||
return xxxx * m[0] + yyyy * m[1] + zzzz * m[2];
|
||||
}
|
||||
|
||||
inline const Vector3 operator*(const Matrix33& m, const Vector3& vT)
|
||||
{
|
||||
return Vector3(Dot(m[0], vT), Dot(m[1], vT), Dot(m[2], vT));
|
||||
}
|
||||
|
||||
inline void Matrix33::operator*=(const Matrix33& a)
|
||||
{
|
||||
*this = *this * a;
|
||||
}
|
||||
|
||||
inline void Matrix33::operator*=(const Scalar& s)
|
||||
{
|
||||
m_rows[0] *= s;
|
||||
m_rows[1] *= s;
|
||||
m_rows[2] *= s;
|
||||
}
|
||||
|
||||
inline void Matrix33::operator+=(const Matrix33& a)
|
||||
{
|
||||
m_rows[0] += a[0];
|
||||
m_rows[1] += a[1];
|
||||
m_rows[2] += a[2];
|
||||
}
|
||||
|
||||
inline void Matrix33::operator-=(const Matrix33& a)
|
||||
{
|
||||
m_rows[0] -= a[0];
|
||||
m_rows[1] -= a[1];
|
||||
m_rows[2] -= a[2];
|
||||
}
|
||||
|
||||
inline const Matrix33 operator*(const Scalar& s, const Matrix33& m)
|
||||
{
|
||||
Vector3 scale(s);
|
||||
return Matrix33(scale * m[0], scale * m[1], scale * m[2]);
|
||||
}
|
||||
|
||||
inline const Matrix33 operator*(const Matrix33& m, const Scalar& s)
|
||||
{
|
||||
Vector3 scale(s);
|
||||
return Matrix33(scale * m[0], scale * m[1], scale * m[2]);
|
||||
}
|
||||
|
||||
inline const Matrix33 operator*(const Matrix33& a, const Matrix33& b)
|
||||
{
|
||||
return Matrix33(a[0] * b, a[1] * b, a[2] * b);
|
||||
}
|
||||
|
||||
inline const Matrix33 operator+(const Matrix33& a, const Matrix33& b)
|
||||
{
|
||||
return Matrix33(a[0] + b[0], a[1] + b[1], a[2] + b[2]);
|
||||
}
|
||||
|
||||
inline const Matrix33 operator-(const Matrix33& a, const Matrix33& b)
|
||||
{
|
||||
return Matrix33(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
|
||||
}
|
||||
|
||||
inline const Matrix33 Transpose(const Matrix33& m)
|
||||
{
|
||||
// easiest way is to actually do a 4 * 4 transpose with an implied zero bottom row:
|
||||
|
||||
// a b c d a e i 0
|
||||
// e f g h ---> b f j 0
|
||||
// i j k l c g k 0
|
||||
// 0 0 0 0
|
||||
|
||||
// shuffle the rows to make 4 quarters
|
||||
__m128 abef = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(1, 0, 1, 0));
|
||||
__m128 cdgh = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(3, 2, 3, 2));
|
||||
__m128 ij00 = _mm_shuffle_ps(m[2].base, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 1, 0));
|
||||
__m128 kl00 = _mm_shuffle_ps(m[2].base, _mm_setzero_ps(), _MM_SHUFFLE(3, 2, 3, 2));
|
||||
|
||||
// shuffle the quarters to make new rows
|
||||
__m128 aei0 = _mm_shuffle_ps(abef, ij00, _MM_SHUFFLE(2, 0, 2, 0));
|
||||
__m128 bfj0 = _mm_shuffle_ps(abef, ij00, _MM_SHUFFLE(3, 1, 3, 1));
|
||||
__m128 cgk0 = _mm_shuffle_ps(cdgh, kl00, _MM_SHUFFLE(2, 0, 2, 0));
|
||||
|
||||
return Matrix33(Vector3(aei0), Vector3(bfj0), Vector3(cgk0));
|
||||
}
|
||||
|
||||
inline const Scalar Det(const Matrix33& m)
|
||||
{
|
||||
return Dot(Cross(m.GetAxisX(), m.GetAxisY()),m.GetAxisZ());
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Matrix44
|
||||
|
||||
inline Matrix44::Matrix44()
|
||||
{
|
||||
}
|
||||
|
||||
inline Matrix44::Matrix44(const Vector4& x, const Vector4& y, const Vector4& z, const Vector4& w)
|
||||
{
|
||||
m_rows[0] = x;
|
||||
m_rows[1] = y;
|
||||
m_rows[2] = z;
|
||||
m_rows[3] = w;
|
||||
}
|
||||
|
||||
inline Matrix44::Matrix44(const Maths::ZeroTag&)
|
||||
{
|
||||
m_rows[0] = Maths::Zero;
|
||||
m_rows[1] = Maths::Zero;
|
||||
m_rows[2] = Maths::Zero;
|
||||
m_rows[3] = Maths::Zero;
|
||||
}
|
||||
|
||||
inline Matrix44::Matrix44(const Maths::IdentityTag&)
|
||||
{
|
||||
m_rows[0] = Maths::UnitX;
|
||||
m_rows[1] = Maths::UnitY;
|
||||
m_rows[2] = Maths::UnitZ;
|
||||
m_rows[3] = Maths::UnitW;
|
||||
}
|
||||
|
||||
inline Matrix44::Matrix44(const Maths::RotateXTag&, float radians)
|
||||
{
|
||||
float c = cosf(radians);
|
||||
float s = sinf(radians);
|
||||
|
||||
m_rows[0] = Maths::UnitX;
|
||||
m_rows[1] = Vector4(0.0f, c, s, 0.0f);
|
||||
m_rows[2] = Vector4(0.0f, -s, c, 0.0f);
|
||||
m_rows[3] = Maths::UnitW;
|
||||
}
|
||||
|
||||
inline Matrix44::Matrix44(const Maths::RotateYTag&, float radians)
|
||||
{
|
||||
float c = cosf(radians);
|
||||
float s = sinf(radians);
|
||||
|
||||
m_rows[0] = Vector4(c, 0.0f, -s, 0.0f);
|
||||
m_rows[1] = Maths::UnitY;
|
||||
m_rows[2] = Vector4(s, 0.0f, c, 0.0f);
|
||||
m_rows[3] = Maths::UnitW;
|
||||
}
|
||||
|
||||
inline Matrix44::Matrix44(const Maths::RotateZTag&, float radians)
|
||||
{
|
||||
float c = cosf(radians);
|
||||
float s = sinf(radians);
|
||||
|
||||
m_rows[0] = Vector4(c, s, 0.0f, 0.0f);
|
||||
m_rows[1] = Vector4(-s, c, 0.0f, 0.0f);
|
||||
m_rows[2] = Maths::UnitZ;
|
||||
m_rows[3] = Maths::UnitW;
|
||||
}
|
||||
|
||||
inline const Matrix44& Matrix44::operator=(const Matrix44& m)
|
||||
{
|
||||
m_rows[0] = m.m_rows[0];
|
||||
m_rows[1] = m.m_rows[1];
|
||||
m_rows[2] = m.m_rows[2];
|
||||
m_rows[3] = m.m_rows[3];
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Matrix44& Matrix44::operator=(const Maths::ZeroTag&)
|
||||
{
|
||||
m_rows[0] = m_rows[1] = m_rows[2] = m_rows[3] = Maths::Zero;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Matrix44& Matrix44::operator=(const Maths::IdentityTag&)
|
||||
{
|
||||
m_rows[0] = Maths::UnitX;
|
||||
m_rows[1] = Maths::UnitY;
|
||||
m_rows[2] = Maths::UnitZ;
|
||||
m_rows[3] = Maths::UnitW;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline Vector4& Matrix44::operator[](int row)
|
||||
{
|
||||
return m_rows[row];
|
||||
}
|
||||
|
||||
inline const Vector4& Matrix44::operator[](int row) const
|
||||
{
|
||||
return m_rows[row];
|
||||
}
|
||||
|
||||
inline void Matrix44::operator*=(const Matrix44& a)
|
||||
{
|
||||
*this = *this * a;
|
||||
}
|
||||
|
||||
inline void Matrix44::operator*=(const Scalar& s)
|
||||
{
|
||||
m_rows[0] *= s;
|
||||
m_rows[1] *= s;
|
||||
m_rows[2] *= s;
|
||||
m_rows[3] *= s;
|
||||
}
|
||||
|
||||
inline void Matrix44::operator+=(const Matrix44& a)
|
||||
{
|
||||
m_rows[0] += a[0];
|
||||
m_rows[1] += a[1];
|
||||
m_rows[2] += a[2];
|
||||
m_rows[3] += a[3];
|
||||
}
|
||||
|
||||
inline void Matrix44::operator-=(const Matrix44& a)
|
||||
{
|
||||
m_rows[0] -= a[0];
|
||||
m_rows[1] -= a[1];
|
||||
m_rows[2] -= a[2];
|
||||
m_rows[3] -= a[3];
|
||||
}
|
||||
|
||||
inline const Vector3 operator*(const Vector3& v, const Matrix44& m)
|
||||
{
|
||||
Scalar xxxx = v.GetX();
|
||||
Scalar yyyy = v.GetY();
|
||||
Scalar zzzz = v.GetZ();
|
||||
|
||||
return xxxx * Vector3(m[0]) + yyyy * Vector3(m[1]) + zzzz * Vector3(m[2]);
|
||||
}
|
||||
|
||||
inline const Point3 operator*(const Point3& v, const Matrix44& m)
|
||||
{
|
||||
Scalar xxxx = v.GetX();
|
||||
Scalar yyyy = v.GetY();
|
||||
Scalar zzzz = v.GetZ();
|
||||
|
||||
return Point3(xxxx * m[0] + yyyy * m[1] + zzzz * m[2] + m[3]);
|
||||
}
|
||||
|
||||
inline const Vector4 operator*(const Vector4& v, const Matrix44& m)
|
||||
{
|
||||
Scalar xxxx = v.GetX();
|
||||
Scalar yyyy = v.GetY();
|
||||
Scalar zzzz = v.GetZ();
|
||||
Scalar wwww = v.GetW();
|
||||
|
||||
return xxxx * m[0] + yyyy * m[1] + zzzz * m[2] + wwww * m[3];
|
||||
}
|
||||
|
||||
inline const Matrix44 operator*(const Matrix44& a, const Matrix44& b)
|
||||
{
|
||||
return Matrix44(a[0] * b, a[1] * b, a[2] * b, a[3] * b);
|
||||
}
|
||||
|
||||
inline const Matrix44 operator*(const Matrix44& m, const Scalar& s)
|
||||
{
|
||||
Vector4 scale(s);
|
||||
return Matrix44(scale * m[0], scale * m[1], scale * m[2], scale * m[3]);
|
||||
}
|
||||
|
||||
inline const Matrix44 operator*(const Scalar& s, const Matrix44& m)
|
||||
{
|
||||
Vector4 scale(s);
|
||||
return Matrix44(scale * m[0], scale * m[1], scale * m[2], scale * m[3]);
|
||||
}
|
||||
|
||||
inline const Matrix44 operator+(const Matrix44& a, const Matrix44& b)
|
||||
{
|
||||
return Matrix44(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]);
|
||||
}
|
||||
|
||||
inline const Matrix44 operator-(const Matrix44& a, const Matrix44& b)
|
||||
{
|
||||
return Matrix44(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]);
|
||||
}
|
||||
|
||||
inline const Matrix44 Transpose(const Matrix44& m)
|
||||
{
|
||||
// a b c d a e i m
|
||||
// e f g h ---> b f j n
|
||||
// i j k l c g k o
|
||||
// m n o p d h l p
|
||||
|
||||
// shuffle the rows to make 4 quarters
|
||||
__m128 abef = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(1, 0, 1, 0));
|
||||
__m128 cdgh = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(3, 2, 3, 2));
|
||||
__m128 ijmn = _mm_shuffle_ps(m[2].base, m[3].base, _MM_SHUFFLE(1, 0, 1, 0));
|
||||
__m128 klop = _mm_shuffle_ps(m[2].base, m[3].base, _MM_SHUFFLE(3, 2, 3, 2));
|
||||
|
||||
// shuffle the quarters to make new rows
|
||||
__m128 aeim = _mm_shuffle_ps(abef, ijmn, _MM_SHUFFLE(2, 0, 2, 0));
|
||||
__m128 bfjn = _mm_shuffle_ps(abef, ijmn, _MM_SHUFFLE(3, 1, 3, 1));
|
||||
__m128 cgko = _mm_shuffle_ps(cdgh, klop, _MM_SHUFFLE(2, 0, 2, 0));
|
||||
__m128 dhlp = _mm_shuffle_ps(cdgh, klop, _MM_SHUFFLE(3, 1, 3, 1));
|
||||
|
||||
return Matrix44(Vector4(aeim), Vector4(bfjn), Vector4(cgko), Vector4(dhlp));
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Transform
|
||||
|
||||
inline Transform::Transform()
|
||||
{
|
||||
}
|
||||
|
||||
inline Transform::Transform(const Matrix33& xyz, const Point3& w)
|
||||
{
|
||||
m_rotation = xyz;
|
||||
m_translation = w;
|
||||
}
|
||||
|
||||
inline Transform::Transform(const Vector3& x, const Vector3& y, const Vector3& z, const Point3& w)
|
||||
{
|
||||
m_rotation[0] = x;
|
||||
m_rotation[1] = y;
|
||||
m_rotation[2] = z;
|
||||
m_translation = w;
|
||||
}
|
||||
|
||||
inline Transform::Transform(const Maths::IdentityTag&)
|
||||
{
|
||||
m_rotation = Maths::Identity;
|
||||
m_translation = Maths::Zero;
|
||||
}
|
||||
|
||||
inline const Transform& Transform::operator=(const Transform& m)
|
||||
{
|
||||
m_rotation = m.m_rotation;
|
||||
m_translation = m.m_translation;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Transform& Transform::operator=(const Maths::IdentityTag&)
|
||||
{
|
||||
m_rotation = Maths::Identity;
|
||||
m_translation = Maths::Zero;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline Matrix33& Transform::GetRotation()
|
||||
{
|
||||
return m_rotation;
|
||||
}
|
||||
|
||||
inline const Matrix33& Transform::GetRotation() const
|
||||
{
|
||||
return m_rotation;
|
||||
}
|
||||
|
||||
inline void Transform::SetRotation(const Matrix33& m)
|
||||
{
|
||||
m_rotation = m;
|
||||
}
|
||||
|
||||
inline void Transform::SetRotation(const Quat& q)
|
||||
{
|
||||
m_rotation = Matrix33(q);
|
||||
}
|
||||
|
||||
inline Point3& Transform::GetTranslation()
|
||||
{
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
inline const Point3& Transform::GetTranslation() const
|
||||
{
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
inline void Transform::SetTranslation(const Point3& t)
|
||||
{
|
||||
m_translation = t;
|
||||
}
|
||||
|
||||
inline Vector3& Transform::GetAxisX()
|
||||
{
|
||||
return m_rotation[0];
|
||||
}
|
||||
|
||||
inline const Vector3& Transform::GetAxisX() const
|
||||
{
|
||||
return m_rotation[0];
|
||||
}
|
||||
|
||||
inline Vector3& Transform::GetAxisY()
|
||||
{
|
||||
return m_rotation[1];
|
||||
}
|
||||
|
||||
inline const Vector3& Transform::GetAxisY() const
|
||||
{
|
||||
return m_rotation[1];
|
||||
}
|
||||
|
||||
inline Vector3& Transform::GetAxisZ()
|
||||
{
|
||||
return m_rotation[2];
|
||||
}
|
||||
|
||||
inline const Vector3& Transform::GetAxisZ() const
|
||||
{
|
||||
return m_rotation[2];
|
||||
}
|
||||
|
||||
inline const Vector3 operator*(const Vector3& v, const Transform& m)
|
||||
{
|
||||
Scalar xxxx = v.GetX();
|
||||
Scalar yyyy = v.GetY();
|
||||
Scalar zzzz = v.GetZ();
|
||||
|
||||
return xxxx * m.GetAxisX() + yyyy * m.GetAxisY() + zzzz * m.GetAxisZ();
|
||||
}
|
||||
|
||||
inline const Point3 operator*(const Point3& v, const Transform& m)
|
||||
{
|
||||
Scalar xxxx = v.GetX();
|
||||
Scalar yyyy = v.GetY();
|
||||
Scalar zzzz = v.GetZ();
|
||||
|
||||
return xxxx * m.GetAxisX() + yyyy * m.GetAxisY() + zzzz * m.GetAxisZ() + m.GetTranslation();
|
||||
}
|
||||
|
||||
inline const Transform operator*(const Transform& a, const Transform& b)
|
||||
{
|
||||
return Transform(a.GetAxisX() * b, a.GetAxisY() * b, a.GetAxisZ() * b, a.GetTranslation() * b);
|
||||
}
|
||||
|
||||
inline const Transform Inv(const Transform& m)
|
||||
{
|
||||
Matrix33 r(Transpose(m.GetRotation()));
|
||||
Point3 t = m.GetTranslation();
|
||||
t = Point3(-(t.GetX() * r.GetAxisX() + t.GetY() * r.GetAxisY() + t.GetZ() * r.GetAxisZ()));
|
||||
|
||||
return Transform(r, t);
|
||||
}
|
||||
63
Extras/obsolete/SATConvexCollision/Memory2.h
Normal file
63
Extras/obsolete/SATConvexCollision/Memory2.h
Normal file
@@ -0,0 +1,63 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Memory.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
#ifndef BULLET_MEMORY2_H
|
||||
#define BULLET_MEMORY2_H
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
//added __cdecl, thanks Jack
|
||||
|
||||
// default new and delete overrides that guarantee 16 byte alignment and zero allocated memory
|
||||
void* __cdecl operator new(size_t sz) throw();
|
||||
void* __cdecl operator new[](size_t sz) throw();
|
||||
void __cdecl operator delete(void* m) throw();
|
||||
void __cdecl operator delete[](void* m) throw();
|
||||
|
||||
#include <malloc.h>
|
||||
|
||||
#define BULLET_ALIGNED_NEW_AND_DELETE \
|
||||
\
|
||||
inline void* operator new(size_t sz) throw() \
|
||||
{ \
|
||||
void* mem = _aligned_malloc(sz + 64, 16); \
|
||||
return mem; \
|
||||
} \
|
||||
\
|
||||
inline void* operator new[](size_t sz) throw() \
|
||||
{ \
|
||||
void* mem = _aligned_malloc(sz + 64, 16); \
|
||||
return mem; \
|
||||
} \
|
||||
\
|
||||
inline void operator delete(void* m) throw() \
|
||||
{ \
|
||||
if (m == 0) \
|
||||
return; \
|
||||
_aligned_free(m); \
|
||||
} \
|
||||
\
|
||||
inline void operator delete[](void* m) throw() \
|
||||
{ \
|
||||
_aligned_free(m); \
|
||||
} \
|
||||
|
||||
|
||||
|
||||
#endif //WIN32
|
||||
|
||||
#endif //MEMORY2_H
|
||||
108
Extras/obsolete/SATConvexCollision/Quat.cpp
Normal file
108
Extras/obsolete/SATConvexCollision/Quat.cpp
Normal file
@@ -0,0 +1,108 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Quat.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
|
||||
#include "Quat.h"
|
||||
#include "Maths.h"
|
||||
|
||||
bool Quat::IsFinite() const
|
||||
{
|
||||
if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()) && _finite(GetW()))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
Quat::Quat(const Matrix33& m)
|
||||
{
|
||||
float tr, s, q[4];
|
||||
int i, j, k;
|
||||
int nxt[3] = {1, 2, 0};
|
||||
float x, y, z, w;
|
||||
|
||||
// Check the diagonal
|
||||
tr = m[0][0] + m[1][1] + m[2][2];
|
||||
if (tr >= 0.0f)
|
||||
{
|
||||
// Diagonal is positive
|
||||
s = ::Sqrt(tr + 1.0f);
|
||||
w = s * 0.5f;
|
||||
s = 0.5f / s;
|
||||
x = (m[1][2] - m[2][1]) * s;
|
||||
y = (m[2][0] - m[0][2]) * s;
|
||||
z = (m[0][1] - m[1][0]) * s;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Diagonal is negative
|
||||
i = 0;
|
||||
if (m[1][1] > m[0][0]) i = 1;
|
||||
if (m[2][2] > m[i][i]) i = 2;
|
||||
j = nxt[i];
|
||||
k = nxt[j];
|
||||
|
||||
s = ::Sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
|
||||
q[i] = s * 0.5f;
|
||||
if (s != 0.0f) s = 0.5f / s;
|
||||
|
||||
q[3] = (m[j][k] - m[k][j]) * s;
|
||||
q[j] = (m[i][j] + m[j][i]) * s;
|
||||
q[k] = (m[i][k] + m[k][i]) * s;
|
||||
|
||||
x = q[0];
|
||||
y = q[1];
|
||||
z = q[2];
|
||||
w = q[3];
|
||||
}
|
||||
|
||||
*this = Quat(x, y, z, w);
|
||||
}
|
||||
|
||||
const Quat Slerp(const Quat& a, const Quat& b, const Scalar& t)
|
||||
{
|
||||
Quat e;
|
||||
Scalar cosom, t0, t1;
|
||||
|
||||
cosom = Dot(a, b);
|
||||
|
||||
if (cosom < Scalar::Consts::Zero)
|
||||
{
|
||||
cosom = -cosom;
|
||||
e = -b;
|
||||
}
|
||||
else
|
||||
e = b;
|
||||
|
||||
if (cosom < 0.9999f)
|
||||
{
|
||||
float omega = ::Acos(cosom);
|
||||
Scalar rcpSinom = Rcp(Scalar(::Sin(omega)));
|
||||
t0 = Scalar(::Sin((1.0f - (float)t) * omega)) * rcpSinom;
|
||||
t1 = Scalar(::Sin((float)t * omega)) * rcpSinom;
|
||||
}
|
||||
else
|
||||
{
|
||||
t0 = Scalar(1.0f) - t;
|
||||
t1 = t;
|
||||
}
|
||||
|
||||
return a * t0 + e * t;
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif //#ifdef WIN32
|
||||
87
Extras/obsolete/SATConvexCollision/Quat.h
Normal file
87
Extras/obsolete/SATConvexCollision/Quat.h
Normal file
@@ -0,0 +1,87 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Quat.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef BULLET_QUAT_H
|
||||
#define BULLET_QUAT_H
|
||||
|
||||
#include "Vector.h"
|
||||
|
||||
class Matrix33;
|
||||
|
||||
|
||||
class Quat : public Vector4Base
|
||||
{
|
||||
public:
|
||||
BULLET_ALIGNED_NEW_AND_DELETE
|
||||
|
||||
// constructors
|
||||
Quat();
|
||||
Quat(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w);
|
||||
Quat(const Vector3& axis, const Scalar& angle);
|
||||
|
||||
// construction to constant
|
||||
Quat(const Maths::IdentityTag&);
|
||||
|
||||
// explicit constructors
|
||||
explicit Quat(const __m128 b);
|
||||
explicit Quat(const Vector3& v);
|
||||
explicit Quat(const Vector4& v);
|
||||
explicit Quat(const Matrix33& m);
|
||||
explicit Quat(const float* p);
|
||||
|
||||
// assignment
|
||||
const Quat& operator=(const Quat& v);
|
||||
const Quat& operator=(const Maths::IdentityTag&);
|
||||
|
||||
// in place operations
|
||||
void operator+=(const Quat& b);
|
||||
void operator-=(const Quat& b);
|
||||
void operator*=(const Quat& b);
|
||||
void operator*=(const Scalar& s);
|
||||
|
||||
// operations
|
||||
friend const Quat operator-(const Quat& a);
|
||||
friend const Quat operator+(const Quat& a, const Quat& b);
|
||||
friend const Quat operator-(const Quat& a, const Quat& b);
|
||||
friend const Quat operator*(const Quat& a, const Quat& b);
|
||||
friend const Quat operator*(const Quat& a, const Scalar& s);
|
||||
friend const Quat operator*(const Scalar& s, const Quat& a);
|
||||
|
||||
friend const Quat Inv(const Quat& a);
|
||||
|
||||
friend const Scalar Dot(const Quat& a, const Quat& b);
|
||||
|
||||
friend const Scalar Length(const Quat& a);
|
||||
friend const Quat Normalize(const Quat& a);
|
||||
|
||||
friend const Scalar LengthFast(const Quat& a);
|
||||
friend const Quat NormalizeFast(const Quat& a);
|
||||
|
||||
friend const Quat Slerp(const Quat& a, const Quat& b, const Scalar& t);
|
||||
friend const Quat Lerp(const Quat& a, const Quat& b, const Scalar& t);
|
||||
|
||||
const Vector3 Rotate(const Vector3& v) const;
|
||||
void GetAngleAxis(Vector3& axis, Scalar& angle) const;
|
||||
|
||||
// validation
|
||||
bool IsFinite() const;
|
||||
};
|
||||
|
||||
|
||||
#include "Quat.inl"
|
||||
|
||||
|
||||
#endif //BULLET_QUAT_H
|
||||
189
Extras/obsolete/SATConvexCollision/Quat.inl
Normal file
189
Extras/obsolete/SATConvexCollision/Quat.inl
Normal file
@@ -0,0 +1,189 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Quat.inl
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#pragma once
|
||||
|
||||
#include <assert.h>
|
||||
#include "math.h"
|
||||
|
||||
inline Quat::Quat()
|
||||
{
|
||||
}
|
||||
|
||||
inline Quat::Quat(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
|
||||
{
|
||||
Set(x.base, y.base, z.base, w.base);
|
||||
}
|
||||
|
||||
inline Quat::Quat(const Vector3& axis, const Scalar& angle)
|
||||
{
|
||||
assert(axis.IsFinite());
|
||||
Set((sinf(0.5f * (float)angle) * axis).base, Scalar(cosf(0.5f * (float)angle)).base);
|
||||
}
|
||||
|
||||
inline Quat::Quat(const Maths::IdentityTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k0001;
|
||||
}
|
||||
|
||||
inline Quat::Quat(const Vector3& v)
|
||||
{
|
||||
base = v.base;
|
||||
}
|
||||
|
||||
inline Quat::Quat(const Vector4& v)
|
||||
{
|
||||
base = v.base;
|
||||
}
|
||||
|
||||
inline Quat::Quat(const __m128 b)
|
||||
{
|
||||
base = b;
|
||||
}
|
||||
|
||||
inline Quat::Quat(const float* p)
|
||||
{
|
||||
base = _mm_load_ps(p);
|
||||
}
|
||||
|
||||
inline const Quat& Quat::operator=(const Quat &q)
|
||||
{
|
||||
base = q.base;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline const Quat& Quat::operator=(const Maths::IdentityTag&)
|
||||
{
|
||||
base = Vector4Base::Consts::k0001;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline void Quat::operator+=(const Quat& b)
|
||||
{
|
||||
base = _mm_add_ps(base, b.base);
|
||||
}
|
||||
|
||||
inline void Quat::operator-=(const Quat& b)
|
||||
{
|
||||
base = _mm_sub_ps(base, b.base);
|
||||
}
|
||||
|
||||
inline void Quat::operator*=(const Quat& b)
|
||||
{
|
||||
*this = (*this * b);
|
||||
}
|
||||
|
||||
inline void Quat::operator*=(const Scalar& s)
|
||||
{
|
||||
base = _mm_mul_ps(base, _mm_shuffle_ps(s.base, s.base, _MM_SHUFFLE(0, 0, 0, 0)));
|
||||
}
|
||||
|
||||
inline const Quat operator-(const Quat& a)
|
||||
{
|
||||
return Quat(_mm_sub_ps(_mm_setzero_ps(), a.base));
|
||||
}
|
||||
|
||||
inline const Quat operator+(const Quat& a, const Quat& b)
|
||||
{
|
||||
return Quat(_mm_add_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Quat operator-(const Quat& a, const Quat& b)
|
||||
{
|
||||
return Quat(_mm_sub_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Quat operator*(const Quat& a, const Quat& b)
|
||||
{
|
||||
// TODO: not happy with this
|
||||
Vector3 va(a.base);
|
||||
Vector3 vb(b.base);
|
||||
Scalar wa(va.GetW());
|
||||
Scalar wb(vb.GetW());
|
||||
return Quat(Vector4(Cross(va, vb) + (va * wb) + (vb * wa), (wa * wb) - Dot(va, vb)));
|
||||
}
|
||||
|
||||
inline const Quat operator*(const Quat& a, const Scalar& s)
|
||||
{
|
||||
return Quat(_mm_mul_ps(a.base, _mm_shuffle_ps(s.base, s.base, _MM_SHUFFLE(0, 0, 0, 0))));
|
||||
}
|
||||
|
||||
inline const Quat operator*(const Scalar& s, const Quat& a)
|
||||
{
|
||||
return Quat(_mm_mul_ps(a.base, _mm_shuffle_ps(s.base, s.base, _MM_SHUFFLE(0, 0, 0, 0))));
|
||||
}
|
||||
|
||||
inline const Quat Inv(const Quat& a)
|
||||
{
|
||||
return Quat(_mm_mul_ps(a.base, Vector4Base::Consts::kNeg111_1));
|
||||
}
|
||||
|
||||
inline const Scalar Dot(const Quat& a, const Quat& b)
|
||||
{
|
||||
return Scalar(Vector4Base::Dot4(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar Length(const Quat& a)
|
||||
{
|
||||
return RsqrtNr(Dot(a, a));
|
||||
}
|
||||
|
||||
inline const Quat Normalize(const Quat& a)
|
||||
{
|
||||
return a * RsqrtNr(Dot(a, a));
|
||||
}
|
||||
|
||||
inline const Scalar LengthFast(const Quat& a)
|
||||
{
|
||||
return Rsqrt(Dot(a, a));
|
||||
}
|
||||
|
||||
inline const Quat NormalizeFast(const Quat& a)
|
||||
{
|
||||
return a * Rsqrt(Dot(a, a));
|
||||
}
|
||||
|
||||
inline const Quat Lerp(const Quat& a, const Quat& b, const Scalar& t)
|
||||
{
|
||||
Quat e;
|
||||
|
||||
// go the shortest route
|
||||
if (IsNegative(Dot(a, b)))
|
||||
e = -b;
|
||||
else
|
||||
e = b;
|
||||
|
||||
return Normalize(a + (e - a) * t);
|
||||
}
|
||||
|
||||
inline const Vector3 Quat::Rotate(const Vector3& v) const
|
||||
{
|
||||
return Vector3(*this * Quat(v) * Inv(*this));
|
||||
}
|
||||
|
||||
inline void Quat::GetAngleAxis(Vector3& axis, Scalar& angle) const
|
||||
{
|
||||
float cosa = GetW();
|
||||
|
||||
angle = acosf(cosa);
|
||||
angle += angle; // * 2;
|
||||
|
||||
float sina = sqrtf(1.0f - cosa * cosa);
|
||||
|
||||
// if ( fabs( sina ) < 0.0005 ) sina = 1;
|
||||
|
||||
axis = RcpNr(sina) * Vector3(*this);
|
||||
}
|
||||
39
Extras/obsolete/SATConvexCollision/Scalar.cpp
Normal file
39
Extras/obsolete/SATConvexCollision/Scalar.cpp
Normal file
@@ -0,0 +1,39 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Scalar.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
|
||||
#include "Scalar.h"
|
||||
|
||||
const Scalar Scalar::Consts::MinusOne(-1.0f);
|
||||
const Scalar Scalar::Consts::Zero(0.0f);
|
||||
const Scalar Scalar::Consts::Half(0.5f);
|
||||
const Scalar Scalar::Consts::One(1.0f);
|
||||
const Scalar Scalar::Consts::Three(3.0f);
|
||||
|
||||
const Scalar Scalar::Consts::MinValue(-3.402823466e+38f);
|
||||
const Scalar Scalar::Consts::MaxValue(3.402823466e+38f);
|
||||
const Scalar Scalar::Consts::Epsilon(1.192092896e-07f);
|
||||
|
||||
const Scalar Scalar::Consts::PosInfinity(0x7f800000, true);
|
||||
const Scalar Scalar::Consts::NegInfinity(0xff800000, true);
|
||||
|
||||
const Scalar Scalar::Consts::AbsMask(0x7fffffff, true);
|
||||
|
||||
#endif
|
||||
#endif //WIN32
|
||||
|
||||
106
Extras/obsolete/SATConvexCollision/Scalar.h
Normal file
106
Extras/obsolete/SATConvexCollision/Scalar.h
Normal file
@@ -0,0 +1,106 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Scalar.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef BULLET_SCALAR_H
|
||||
#define BULLET_SCALAR_H
|
||||
|
||||
|
||||
#include <xmmintrin.h>
|
||||
#include "Memory2.h"
|
||||
|
||||
// resolved overload found with Koenig lookup
|
||||
#pragma warning (disable : 4675)
|
||||
|
||||
|
||||
__declspec(align(16)) class Scalar
|
||||
{
|
||||
public:
|
||||
BULLET_ALIGNED_NEW_AND_DELETE
|
||||
|
||||
__m128 base;
|
||||
|
||||
// constants
|
||||
struct Consts{
|
||||
static const Scalar
|
||||
MinusOne,
|
||||
Zero,
|
||||
Half,
|
||||
One,
|
||||
Three,
|
||||
MinValue,
|
||||
MaxValue,
|
||||
Epsilon,
|
||||
NegInfinity,
|
||||
PosInfinity,
|
||||
AbsMask;
|
||||
};
|
||||
|
||||
// constructors
|
||||
Scalar();
|
||||
Scalar(float f);
|
||||
Scalar(int i, bool forceNoConvert);
|
||||
|
||||
// explicit constructors
|
||||
explicit Scalar(__m128 s);
|
||||
explicit Scalar(int i);
|
||||
|
||||
// assignment
|
||||
const Scalar& operator=(const Scalar& a);
|
||||
|
||||
// conversion
|
||||
operator const float() const;
|
||||
operator const float();
|
||||
|
||||
// in place operations
|
||||
void operator+=(const Scalar& b);
|
||||
void operator-=(const Scalar& b);
|
||||
void operator*=(const Scalar& b);
|
||||
void operator/=(const Scalar& b);
|
||||
|
||||
// operations
|
||||
friend const Scalar operator-(const Scalar& a);
|
||||
|
||||
friend const Scalar operator+(const Scalar& a, const Scalar& b);
|
||||
friend const Scalar operator-(const Scalar& a, const Scalar& b);
|
||||
friend const Scalar operator*(const Scalar& a, const Scalar& b);
|
||||
friend const Scalar operator/(const Scalar& a, const Scalar& b);
|
||||
|
||||
friend const Scalar Abs(const Scalar& a);
|
||||
friend const Scalar Rcp(const Scalar& a);
|
||||
friend const Scalar Rsqrt(const Scalar& a);
|
||||
friend const Scalar Sqrt(const Scalar& a);
|
||||
friend const Scalar RcpNr(const Scalar& a);
|
||||
friend const Scalar RsqrtNr(const Scalar& a);
|
||||
|
||||
friend const Scalar Min(const Scalar& a, const Scalar& b);
|
||||
friend const Scalar Max(const Scalar& a, const Scalar& b);
|
||||
friend const Scalar Clamp(const Scalar& a, const Scalar& min, const Scalar& max);
|
||||
|
||||
friend const Scalar Lerp(const Scalar& a, const Scalar& b, const Scalar& t);
|
||||
|
||||
// comparison
|
||||
friend const int IsNegative(const Scalar& a);
|
||||
|
||||
friend const int IsNan(const Scalar& a);
|
||||
friend const int IsInfinity(const Scalar& a);
|
||||
friend const int IsPosInfinity(const Scalar& a);
|
||||
friend const int IsNegInfinity(const Scalar& a);
|
||||
};
|
||||
|
||||
|
||||
#include "Scalar.inl"
|
||||
|
||||
#endif //BULLET_SCALAR_H
|
||||
196
Extras/obsolete/SATConvexCollision/Scalar.inl
Normal file
196
Extras/obsolete/SATConvexCollision/Scalar.inl
Normal file
@@ -0,0 +1,196 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Scalar.inl
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#pragma once
|
||||
|
||||
|
||||
inline Scalar::Scalar()
|
||||
{
|
||||
}
|
||||
|
||||
inline Scalar::Scalar(float f)
|
||||
{
|
||||
base = _mm_set1_ps(f);
|
||||
}
|
||||
|
||||
|
||||
inline Scalar::Scalar(int i, bool forceNoConvert)
|
||||
{
|
||||
__declspec(align(16)) int iv[4] = {i, i, i, i};
|
||||
*(Scalar*)&base = *(Scalar*)&iv;
|
||||
}
|
||||
|
||||
inline Scalar::Scalar(__m128 s)
|
||||
{
|
||||
base = s;
|
||||
}
|
||||
|
||||
inline Scalar::Scalar(int i)
|
||||
{
|
||||
base = _mm_cvtsi32_ss(base, i);
|
||||
base = _mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
}
|
||||
|
||||
inline const Scalar& Scalar::operator=(const Scalar &a)
|
||||
{
|
||||
base = a.base;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline Scalar::operator const float() const
|
||||
{
|
||||
float f;
|
||||
_mm_store_ss(&f, base);
|
||||
return f;
|
||||
}
|
||||
|
||||
inline Scalar::operator const float()
|
||||
{
|
||||
float f;
|
||||
_mm_store_ss(&f, base);
|
||||
return f;
|
||||
}
|
||||
|
||||
inline void Scalar::operator+=(const Scalar& b)
|
||||
{
|
||||
base = _mm_add_ps(base, b.base);
|
||||
}
|
||||
|
||||
inline void Scalar::operator-=(const Scalar& b)
|
||||
{
|
||||
base = _mm_sub_ps(base, b.base);
|
||||
}
|
||||
|
||||
inline void Scalar::operator*=(const Scalar& b)
|
||||
{
|
||||
base = _mm_mul_ps(base, b.base);
|
||||
}
|
||||
|
||||
inline void Scalar::operator/=(const Scalar& b)
|
||||
{
|
||||
base = _mm_div_ps(base, b.base);
|
||||
}
|
||||
|
||||
inline const Scalar operator-(const Scalar& a)
|
||||
{
|
||||
return Scalar(_mm_sub_ps(_mm_setzero_ps(), a.base));
|
||||
}
|
||||
|
||||
inline const Scalar Abs(const Scalar& a)
|
||||
{
|
||||
return Scalar(_mm_and_ps(a.base, Scalar::Consts::AbsMask.base));
|
||||
}
|
||||
|
||||
inline const Scalar Rcp(const Scalar& a)
|
||||
{
|
||||
return Scalar(_mm_rcp_ps(a.base));
|
||||
}
|
||||
|
||||
inline const Scalar Rsqrt(const Scalar& a)
|
||||
{
|
||||
return Scalar(_mm_rsqrt_ps(a.base));
|
||||
}
|
||||
|
||||
inline const Scalar Sqrt(const Scalar& a)
|
||||
{
|
||||
return Scalar(_mm_sqrt_ps(a.base));
|
||||
}
|
||||
|
||||
// Newton Raphson Reciprocal
|
||||
// (2 * Rcp(x)) - (x * Rcp(x) * Rcp(x))]
|
||||
inline const Scalar RcpNr(const Scalar& a)
|
||||
{
|
||||
Scalar rcp = Rcp(a);
|
||||
return (rcp + rcp) - (a * rcp * rcp);
|
||||
}
|
||||
|
||||
// Newton Raphson Reciprocal Square Root
|
||||
// 0.5 * Rsqrt * (3 - x * Rsqrt(x) * Rsqrt(x))
|
||||
inline const Scalar RsqrtNr(const Scalar& a)
|
||||
{
|
||||
Scalar rcp = Rsqrt(a);
|
||||
return (Scalar::Consts::Half * rcp) * (Scalar::Consts::Three - (a * rcp) * rcp);
|
||||
}
|
||||
|
||||
// binary
|
||||
inline const Scalar operator+(const Scalar& a, const Scalar& b)
|
||||
{
|
||||
return Scalar(_mm_add_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar operator-(const Scalar& a, const Scalar& b)
|
||||
{
|
||||
return Scalar(_mm_sub_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar operator*(const Scalar& a, const Scalar& b)
|
||||
{
|
||||
return Scalar(_mm_mul_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar operator/(const Scalar& a, const Scalar& b)
|
||||
{
|
||||
return Scalar(_mm_div_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar Min(const Scalar& a, const Scalar& b)
|
||||
{
|
||||
return Scalar(_mm_min_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar Max(const Scalar& a, const Scalar& b)
|
||||
{
|
||||
return Scalar(_mm_max_ps(a.base, b.base));
|
||||
}
|
||||
|
||||
inline const Scalar Clamp(const Scalar& a, const Scalar& min, const Scalar& max)
|
||||
{
|
||||
return Scalar(_mm_min_ps(max.base, _mm_max_ps(min.base, a.base)));
|
||||
}
|
||||
|
||||
inline const Scalar Lerp(const Scalar& a, const Scalar& b, const Scalar& t)
|
||||
{
|
||||
return Scalar(a + (b - a) * t);
|
||||
}
|
||||
|
||||
inline const int IsNegative(const Scalar& a)
|
||||
{
|
||||
return _mm_movemask_ps(a.base) & 1;
|
||||
}
|
||||
|
||||
// warning. this only checks for quiet nan
|
||||
inline const int IsNan(const Scalar& a)
|
||||
{
|
||||
int aInt = *(int*)&a;
|
||||
return ((aInt & 0x7fc00000) == 0x7fc00000);
|
||||
}
|
||||
|
||||
inline const int IsInfinity(const Scalar& a)
|
||||
{
|
||||
return (a == Scalar::Consts::PosInfinity || a == Scalar::Consts::NegInfinity);
|
||||
}
|
||||
|
||||
inline const int IsPosInfinity(const Scalar& a)
|
||||
{
|
||||
return (a == Scalar::Consts::PosInfinity);
|
||||
}
|
||||
|
||||
inline const int IsNegInfinity(const Scalar& a)
|
||||
{
|
||||
return (a == Scalar::Consts::NegInfinity);
|
||||
}
|
||||
|
||||
34
Extras/obsolete/SATConvexCollision/Shape.cpp
Normal file
34
Extras/obsolete/SATConvexCollision/Shape.cpp
Normal file
@@ -0,0 +1,34 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Shape.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
|
||||
#include "shape.h"
|
||||
|
||||
Shape::Shape()
|
||||
{
|
||||
}
|
||||
|
||||
Shape::~Shape()
|
||||
{
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif //WIN32
|
||||
67
Extras/obsolete/SATConvexCollision/Shape.h
Normal file
67
Extras/obsolete/SATConvexCollision/Shape.h
Normal file
@@ -0,0 +1,67 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Shape.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
//
|
||||
// Shape.h
|
||||
//
|
||||
#ifndef BULLET_SHAPE_H
|
||||
#define BULLET_SHAPE_H
|
||||
|
||||
#include "Maths.h"
|
||||
|
||||
|
||||
|
||||
|
||||
struct btSeparation
|
||||
{
|
||||
|
||||
short m_featureA;
|
||||
short m_featureB;
|
||||
float m_dist;
|
||||
Vector3 m_axis; // in world space
|
||||
|
||||
// separators
|
||||
enum
|
||||
{
|
||||
kFeatureNone, // not separated
|
||||
kFeatureA,
|
||||
kFeatureB,
|
||||
kFeatureBoth
|
||||
};
|
||||
short m_separator;
|
||||
|
||||
// contact between the 2 bodies (-1 if none)
|
||||
short m_contact;
|
||||
};
|
||||
|
||||
///Shape provides a interface for Hull class (convex hull calculation).
|
||||
class Shape
|
||||
{
|
||||
public:
|
||||
Shape();
|
||||
virtual ~Shape();
|
||||
|
||||
//virtual void ComputeInertia(Point3& centerOfMass, Matrix33& inertia, float totalMass) const = 0;
|
||||
virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const = 0;
|
||||
|
||||
|
||||
virtual Bounds3 ComputeBounds(const Transform& transform) const = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //BULLET_SHAPE_H
|
||||
55
Extras/obsolete/SATConvexCollision/Vector.cpp
Normal file
55
Extras/obsolete/SATConvexCollision/Vector.cpp
Normal file
@@ -0,0 +1,55 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// Vector.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
#include "Vector.h"
|
||||
#include <float.h>
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector3
|
||||
|
||||
bool Vector3::IsFinite() const
|
||||
{
|
||||
if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Point3
|
||||
|
||||
bool Point3::IsFinite() const
|
||||
{
|
||||
if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector4
|
||||
bool Vector4::IsFinite() const
|
||||
{
|
||||
if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()) && _finite(GetW()))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif //WIN32
|
||||
349
Extras/obsolete/SATConvexCollision/Vector.h
Normal file
349
Extras/obsolete/SATConvexCollision/Vector.h
Normal file
@@ -0,0 +1,349 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// Vector.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef BULLET_VECTOR_H
|
||||
#define BULLET_VECTOR_H
|
||||
|
||||
#include "VectorBase.h"
|
||||
|
||||
class Point3;
|
||||
class Vector4;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector3
|
||||
__declspec(align(16)) class Vector3 : public Vector4Base
|
||||
{
|
||||
public:
|
||||
BULLET_ALIGNED_NEW_AND_DELETE
|
||||
|
||||
// constructors
|
||||
Vector3();
|
||||
Vector3(const Vector3& v);
|
||||
Vector3(float x, float y, float z);
|
||||
Vector3(const Scalar& x, const Scalar& y, const Scalar& z);
|
||||
|
||||
// construction to constant
|
||||
Vector3(const Maths::ZeroTag&);
|
||||
Vector3(const Maths::UnitXTag&);
|
||||
Vector3(const Maths::UnitYTag&);
|
||||
Vector3(const Maths::UnitZTag&);
|
||||
Vector3(const Maths::UnitNegXTag&);
|
||||
Vector3(const Maths::UnitNegYTag&);
|
||||
Vector3(const Maths::UnitNegZTag&);
|
||||
|
||||
// explicit constructors
|
||||
explicit Vector3(const __m128 b);
|
||||
explicit Vector3(const Vector4Base& v);
|
||||
explicit Vector3(const Scalar& v);
|
||||
explicit Vector3(const Point3& v);
|
||||
explicit Vector3(const Vector4& v);
|
||||
explicit Vector3(const float* p);
|
||||
|
||||
// assignment
|
||||
const Vector3& operator=(const Vector3& v);
|
||||
|
||||
// assignment to constant
|
||||
const Vector3& operator=(const Maths::ZeroTag&);
|
||||
const Vector3& operator=(const Maths::UnitXTag&);
|
||||
const Vector3& operator=(const Maths::UnitYTag&);
|
||||
const Vector3& operator=(const Maths::UnitZTag&);
|
||||
const Vector3& operator=(const Maths::UnitNegXTag&);
|
||||
const Vector3& operator=(const Maths::UnitNegYTag&);
|
||||
const Vector3& operator=(const Maths::UnitNegZTag&);
|
||||
|
||||
// in place operations
|
||||
void operator+=(const Vector3& b);
|
||||
void operator-=(const Vector3& b);
|
||||
void operator*=(const Vector3& b);
|
||||
void operator/=(const Vector3& b);
|
||||
void operator*=(const Scalar& s);
|
||||
void operator/=(const Scalar& s);
|
||||
|
||||
// operations
|
||||
friend const Vector3 operator-(const Vector3& a);
|
||||
|
||||
friend const Vector3 operator+(const Vector3& a, const Vector3& b);
|
||||
friend const Vector3 operator-(const Vector3& a, const Vector3& b);
|
||||
friend const Vector3 operator*(const Vector3& a, const Vector3& b);
|
||||
friend const Vector3 operator/(const Vector3& a, const Vector3& b);
|
||||
friend const Vector3 operator*(const Vector3& a, const Scalar& s);
|
||||
friend const Vector3 operator*(const Scalar& s, const Vector3& a);
|
||||
friend const Vector3 operator/(const Vector3& a, const Scalar& s);
|
||||
|
||||
friend const Vector3 Abs(const Vector3& a);
|
||||
friend const Vector3 Rcp(const Vector3& a);
|
||||
friend const Vector3 Rsqrt(const Vector3& a);
|
||||
friend const Vector3 Sqrt(const Vector3& a);
|
||||
friend const Vector3 RcpNr(const Vector3& a);
|
||||
friend const Vector3 RsqrtNr(const Vector3& a);
|
||||
|
||||
friend const Scalar Sum(const Vector3& a);
|
||||
friend const Scalar Dot(const Vector3& a, const Vector3& b);
|
||||
|
||||
friend const Vector3 Cross(const Vector3& a, const Vector3& b);
|
||||
|
||||
friend const Vector3 Min(const Vector3& a, const Vector3& b);
|
||||
friend const Vector3 Max(const Vector3& a, const Vector3& b);
|
||||
friend const Vector3 Clamp(const Vector3& v, const Vector3& min, const Vector3& max);
|
||||
|
||||
friend const Scalar MinComp(const Vector3& a);
|
||||
friend const Scalar MaxComp(const Vector3& a);
|
||||
friend const int MinCompIndex(const Vector3& a);
|
||||
friend const int MaxCompIndex(const Vector3& a);
|
||||
|
||||
friend const Scalar Length(const Vector3& a);
|
||||
friend const Scalar LengthSqr(const Vector3& a);
|
||||
friend const Scalar LengthRcp(const Vector3& a);
|
||||
friend const Vector3 Normalize(const Vector3& a);
|
||||
|
||||
friend const Scalar LengthFast(const Vector3& a);
|
||||
friend const Scalar LengthRcpFast(const Vector3& a);
|
||||
friend const Vector3 NormalizeFast(const Vector3& a);
|
||||
|
||||
friend const Vector3 Lerp(const Vector3& a, const Vector3& b, const Scalar& t);
|
||||
|
||||
// returns an arbitrary perpendicular vector (not normalized)
|
||||
friend const Vector3 Perp(const Vector3& v);
|
||||
|
||||
// comparisons (return 1 bit per component)
|
||||
friend int operator==(const Vector3& a, const Vector3& b);
|
||||
friend int operator!=(const Vector3& a, const Vector3& b);
|
||||
friend int operator<(const Vector3& a, const Vector3& b);
|
||||
friend int operator<=(const Vector3& a, const Vector3& b);
|
||||
friend int operator>(const Vector3& a, const Vector3& b);
|
||||
friend int operator>=(const Vector3& a, const Vector3& b);
|
||||
|
||||
// validation
|
||||
bool IsFinite() const;
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Point3
|
||||
__declspec(align(16)) class Point3 : public Vector4Base
|
||||
{
|
||||
public:
|
||||
// constructors
|
||||
Point3();
|
||||
Point3(const Point3& v);
|
||||
Point3(float x, float y, float z);
|
||||
Point3(const Scalar& x, const Scalar& y, const Scalar& z);
|
||||
|
||||
// construction to constant
|
||||
Point3(const Maths::ZeroTag&);
|
||||
Point3(const Maths::UnitXTag&);
|
||||
Point3(const Maths::UnitYTag&);
|
||||
Point3(const Maths::UnitZTag&);
|
||||
Point3(const Maths::UnitNegXTag&);
|
||||
Point3(const Maths::UnitNegYTag&);
|
||||
Point3(const Maths::UnitNegZTag&);
|
||||
|
||||
// explicit constructors
|
||||
explicit Point3(const __m128 b);
|
||||
explicit Point3(const Vector4Base& v);
|
||||
explicit Point3(const Scalar& v);
|
||||
explicit Point3(const Vector3& v);
|
||||
explicit Point3(const Vector4& v);
|
||||
explicit Point3(const float* p);
|
||||
|
||||
// assignment
|
||||
const Point3& operator=(const Point3& v);
|
||||
|
||||
// assignment to constant
|
||||
const Point3& operator=(const Maths::ZeroTag&);
|
||||
const Point3& operator=(const Maths::UnitXTag&);
|
||||
const Point3& operator=(const Maths::UnitYTag&);
|
||||
const Point3& operator=(const Maths::UnitZTag&);
|
||||
const Point3& operator=(const Maths::UnitNegXTag&);
|
||||
const Point3& operator=(const Maths::UnitNegYTag&);
|
||||
const Point3& operator=(const Maths::UnitNegZTag&);
|
||||
|
||||
// in place operations
|
||||
void operator+=(const Vector3& b);
|
||||
void operator-=(const Vector3& b);
|
||||
|
||||
// operations
|
||||
friend const Point3 operator-(const Point3& a);
|
||||
|
||||
friend const Point3 operator+(const Point3& a, const Vector3& b);
|
||||
friend const Point3 operator+(const Vector3& a, const Point3& b);
|
||||
|
||||
friend const Point3 operator-(const Point3& a, const Vector3& b);
|
||||
friend const Vector3 operator-(const Point3& a, const Point3& b);
|
||||
|
||||
friend const Vector3 operator*(const Point3& a, const Vector3& b);
|
||||
friend const Vector3 operator*(const Vector3& a, const Point3& b);
|
||||
|
||||
friend const Point3 Abs(const Point3& a);
|
||||
|
||||
friend const Scalar Sum(const Point3& a);
|
||||
friend const Scalar Dot(const Point3& a, const Point3& b);
|
||||
friend const Scalar Dot(const Vector3& a, const Point3& b);
|
||||
friend const Scalar Dot(const Point3& a, const Vector3& b);
|
||||
|
||||
friend const Point3 Min(const Point3& a, const Point3& b);
|
||||
friend const Point3 Max(const Point3& a, const Point3& b);
|
||||
friend const Point3 Clamp(const Point3& v, const Point3& min, const Point3& max);
|
||||
|
||||
friend const Scalar Dist(const Point3& a, const Point3& b);
|
||||
friend const Scalar DistSqr(const Point3& a, const Point3& b);
|
||||
friend const Scalar DistRcp(const Point3& a, const Point3& b);
|
||||
|
||||
friend const Scalar DistFast(const Point3& a, const Point3& b);
|
||||
friend const Scalar DistRcpFast(const Point3& a, const Point3& b);
|
||||
|
||||
friend const Point3 Lerp(const Point3& a, const Point3& b, const Scalar& t);
|
||||
|
||||
friend const Point3 Homogenize(const Vector4& v);
|
||||
friend const Point3 HomogenizeFast(const Vector4& v);
|
||||
|
||||
// comparisons (return 1 bit per component)
|
||||
friend int operator==(const Point3& a, const Point3& b);
|
||||
friend int operator!=(const Point3& a, const Point3& b);
|
||||
friend int operator<(const Point3& a, const Point3& b);
|
||||
friend int operator<=(const Point3& a, const Point3& b);
|
||||
friend int operator>(const Point3& a, const Point3& b);
|
||||
friend int operator>=(const Point3& a, const Point3& b);
|
||||
|
||||
// validation
|
||||
bool IsFinite() const;
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector4
|
||||
__declspec(align(16)) class Vector4 : public Vector4Base
|
||||
{
|
||||
public:
|
||||
// constructors
|
||||
Vector4();
|
||||
Vector4(const Vector4& v);
|
||||
Vector4(float x, float y, float z, float w);
|
||||
Vector4(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w);
|
||||
Vector4(const Vector3& xyz, const Scalar& w);
|
||||
|
||||
// construction to constant
|
||||
Vector4(const Maths::ZeroTag&);
|
||||
Vector4(const Maths::UnitXTag&);
|
||||
Vector4(const Maths::UnitYTag&);
|
||||
Vector4(const Maths::UnitZTag&);
|
||||
Vector4(const Maths::UnitWTag&);
|
||||
Vector4(const Maths::UnitNegXTag&);
|
||||
Vector4(const Maths::UnitNegYTag&);
|
||||
Vector4(const Maths::UnitNegZTag&);
|
||||
Vector4(const Maths::UnitNegWTag&);
|
||||
|
||||
// explicit constructors
|
||||
explicit Vector4(const __m128 b);
|
||||
explicit Vector4(const Vector4Base& v);
|
||||
explicit Vector4(const Scalar& v);
|
||||
explicit Vector4(const Vector3& v);
|
||||
explicit Vector4(const Point3& v);
|
||||
explicit Vector4(const float* p);
|
||||
|
||||
// assignment
|
||||
const Vector4& operator=(const Vector4& v);
|
||||
|
||||
// assignment to constant
|
||||
const Vector4& operator=(const Maths::ZeroTag&);
|
||||
const Vector4& operator=(const Maths::UnitXTag&);
|
||||
const Vector4& operator=(const Maths::UnitYTag&);
|
||||
const Vector4& operator=(const Maths::UnitZTag&);
|
||||
const Vector4& operator=(const Maths::UnitWTag&);
|
||||
const Vector4& operator=(const Maths::UnitNegXTag&);
|
||||
const Vector4& operator=(const Maths::UnitNegYTag&);
|
||||
const Vector4& operator=(const Maths::UnitNegZTag&);
|
||||
const Vector4& operator=(const Maths::UnitNegWTag&);
|
||||
|
||||
// in place operations
|
||||
void operator+=(const Vector4& b);
|
||||
void operator-=(const Vector4& b);
|
||||
void operator*=(const Vector4& b);
|
||||
void operator/=(const Vector4& b);
|
||||
void operator*=(const Scalar& s);
|
||||
void operator/=(const Scalar& s);
|
||||
|
||||
// operations
|
||||
friend const Vector4 operator-(const Vector4& a);
|
||||
|
||||
friend const Vector4 operator+(const Vector4& a, const Vector4& b);
|
||||
friend const Vector4 operator-(const Vector4& a, const Vector4& b);
|
||||
friend const Vector4 operator*(const Vector4& a, const Vector4& b);
|
||||
friend const Vector4 operator/(const Vector4& a, const Vector4& b);
|
||||
friend const Vector4 operator*(const Vector4& a, const Scalar& s);
|
||||
friend const Vector4 operator*(const Scalar& s, const Vector4& a);
|
||||
friend const Vector4 operator/(const Vector4& a, const Scalar& s);
|
||||
|
||||
friend const Vector4 Abs(const Vector4& a);
|
||||
friend const Vector4 Rcp(const Vector4& a);
|
||||
friend const Vector4 Rsqrt(const Vector4& a);
|
||||
friend const Vector4 Sqrt(const Vector4& a);
|
||||
friend const Vector4 RcpNr(const Vector4& a);
|
||||
friend const Vector4 RsqrtNr(const Vector4& a);
|
||||
|
||||
friend const Scalar Sum(const Vector4& a);
|
||||
friend const Scalar Dot(const Vector4& a, const Vector4& b);
|
||||
|
||||
friend const Vector4 Min(const Vector4& a, const Vector4& b);
|
||||
friend const Vector4 Max(const Vector4& a, const Vector4& b);
|
||||
friend const Vector4 Clamp(const Vector4& v, const Vector4& min, const Vector4& max);
|
||||
|
||||
friend const Scalar MinComp(const Vector4& a);
|
||||
friend const Scalar MaxComp(const Vector4& a);
|
||||
|
||||
friend const Scalar Length(const Vector4& a);
|
||||
friend const Scalar LengthSqr(const Vector4& a);
|
||||
friend const Scalar LengthRcp(const Vector4& a);
|
||||
friend const Vector4 Normalize(const Vector4& a);
|
||||
|
||||
friend const Scalar LengthFast(const Vector4& a);
|
||||
friend const Scalar LengthRcpFast(const Vector4& a);
|
||||
friend const Vector4 NormalizeFast(const Vector4& a);
|
||||
|
||||
friend const Vector4 Lerp(const Vector4& a, const Vector4& b, const Scalar& t);
|
||||
|
||||
// comparisons (return 1 bit per component)
|
||||
friend int operator==(const Vector4& a, const Vector4& b);
|
||||
friend int operator!=(const Vector4& a, const Vector4& b);
|
||||
friend int operator<(const Vector4& a, const Vector4& b);
|
||||
friend int operator<=(const Vector4& a, const Vector4& b);
|
||||
friend int operator>(const Vector4& a, const Vector4& b);
|
||||
friend int operator>=(const Vector4& a, const Vector4& b);
|
||||
|
||||
// validation
|
||||
bool IsFinite() const;
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector2
|
||||
class Vector2 : public Vector2Base
|
||||
{
|
||||
public:
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Point2
|
||||
class Point2 : public Vector2Base
|
||||
{
|
||||
public:
|
||||
|
||||
};
|
||||
|
||||
|
||||
#include "Vector.inl"
|
||||
#endif //BULLET_VECTOR_H
|
||||
1110
Extras/obsolete/SATConvexCollision/Vector.inl
Normal file
1110
Extras/obsolete/SATConvexCollision/Vector.inl
Normal file
File diff suppressed because it is too large
Load Diff
49
Extras/obsolete/SATConvexCollision/VectorBase.cpp
Normal file
49
Extras/obsolete/SATConvexCollision/VectorBase.cpp
Normal file
@@ -0,0 +1,49 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
// VectorBase.cpp
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifdef WIN32
|
||||
#if _MSC_VER >= 1310
|
||||
|
||||
#include "VectorBase.h"
|
||||
|
||||
const __m128 Vector4Base::Consts::kZero = _mm_set1_ps(0.0f);
|
||||
const __m128 Vector4Base::Consts::kHalf = _mm_set1_ps(0.5f);
|
||||
const __m128 Vector4Base::Consts::kThree = _mm_set1_ps(3.0f);
|
||||
|
||||
const __m128 Vector4Base::Consts::k1000 = _mm_setr_ps(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
const __m128 Vector4Base::Consts::k0100 = _mm_setr_ps(0.0f, 1.0f, 0.0f, 0.0f);
|
||||
const __m128 Vector4Base::Consts::k0010 = _mm_setr_ps(0.0f, 0.0f, 1.0f, 0.0f);
|
||||
const __m128 Vector4Base::Consts::k0001 = _mm_setr_ps(0.0f, 0.0f, 0.0f, 1.0f);
|
||||
|
||||
const __m128 Vector4Base::Consts::kNeg1000 = _mm_setr_ps(-1.0f, 0.0f, 0.0f, 0.0f);
|
||||
const __m128 Vector4Base::Consts::kNeg0100 = _mm_setr_ps(0.0f, -1.0f, 0.0f, 0.0f);
|
||||
const __m128 Vector4Base::Consts::kNeg0010 = _mm_setr_ps(0.0f, 0.0f, -1.0f, 0.0f);
|
||||
const __m128 Vector4Base::Consts::kNeg0001 = _mm_setr_ps(0.0f, 0.0f, 0.0f, -1.0f);
|
||||
|
||||
const __m128 Vector4Base::Consts::kNeg111_1 = _mm_setr_ps(-1.0f, -1.0f, -1.0f, 1.0f);
|
||||
const __m128 Vector4Base::Consts::k1110 = _mm_setr_ps(1.0f, 1.0f, 1.0f, 0.0f);
|
||||
|
||||
const unsigned int Vector4Base::Consts::maskAbs = 0x7fffffff;
|
||||
const __m128 Vector4Base::Consts::kMaskAbs = _mm_load1_ps((float*)&Vector4Base::Consts::maskAbs);
|
||||
|
||||
const unsigned int Vector4Base::Consts::mask1110[4] = {0xffffffff, 0xffffffff, 0xffffffff, 0x00000000};
|
||||
const unsigned int Vector4Base::Consts::mask0001[4] = {0x00000000, 0x00000000, 0x00000000, 0xffffffff};
|
||||
|
||||
const __m128 Vector4Base::Consts::kMask1110 = _mm_loadu_ps((float*)Vector4Base::Consts::mask1110);
|
||||
const __m128 Vector4Base::Consts::kMask0001 = _mm_loadu_ps((float*)Vector4Base::Consts::mask0001);
|
||||
|
||||
#endif
|
||||
#endif //WIN32
|
||||
142
Extras/obsolete/SATConvexCollision/VectorBase.h
Normal file
142
Extras/obsolete/SATConvexCollision/VectorBase.h
Normal file
@@ -0,0 +1,142 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// VectorBase.h
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#ifndef BULLET_VECTOR_BASE_H
|
||||
#define BULLET_VECTOR_BASE_H
|
||||
|
||||
#include "Scalar.h"
|
||||
#include "Memory2.h"
|
||||
|
||||
// vector constants
|
||||
namespace Maths
|
||||
{
|
||||
static const enum ZeroTag { } Zero;
|
||||
static const enum UnitXTag { } UnitX;
|
||||
static const enum UnitYTag { } UnitY;
|
||||
static const enum UnitZTag { } UnitZ;
|
||||
static const enum UnitWTag { } UnitW;
|
||||
static const enum UnitNegXTag { } UnitNegX;
|
||||
static const enum UnitNegYTag { } UnitNegY;
|
||||
static const enum UnitNegZTag { } UnitNegZ;
|
||||
static const enum UnitNegWTag { } UnitNegW;
|
||||
|
||||
static const enum IdentityTag { } Identity;
|
||||
static const enum RotateXTag { } RotateX;
|
||||
static const enum RotateYTag { } RotateY;
|
||||
static const enum RotateZTag { } RotateZ;
|
||||
static const enum ScaleTag { } Scale;
|
||||
static const enum SkewTag { } Skew;
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector4Base
|
||||
__declspec(align(16)) class Vector4Base
|
||||
{
|
||||
public:
|
||||
BULLET_ALIGNED_NEW_AND_DELETE
|
||||
|
||||
__m128 base;
|
||||
|
||||
//protected:
|
||||
// useful constants for internal use
|
||||
struct Consts
|
||||
{
|
||||
static const unsigned int maskAbs;
|
||||
static const unsigned int mask1110[4];
|
||||
static const unsigned int mask0001[4];
|
||||
|
||||
static const __m128
|
||||
kZero,
|
||||
kHalf,
|
||||
kThree,
|
||||
|
||||
k1000,
|
||||
k0100,
|
||||
k0010,
|
||||
k0001,
|
||||
|
||||
kNeg1000,
|
||||
kNeg0100,
|
||||
kNeg0010,
|
||||
kNeg0001,
|
||||
|
||||
kNeg111_1,
|
||||
|
||||
k1110,
|
||||
kMaskAbs,
|
||||
kMask1110,
|
||||
kMask0001;
|
||||
};
|
||||
|
||||
// can't construct a Vector4Base
|
||||
Vector4Base();
|
||||
|
||||
// compound operations helpers for use by derived classes
|
||||
void Set(const __m128& x, const __m128& y, const __m128& z, const __m128& w);
|
||||
void Set(const __m128& xyz, const __m128& w);
|
||||
|
||||
static __m128 Dot3(const __m128& v0, const __m128& v1);
|
||||
static __m128 Dot4(const __m128& v0, const __m128& v1);
|
||||
|
||||
static __m128 Sum3(const __m128& a);
|
||||
static __m128 Sum4(const __m128& a);
|
||||
|
||||
static __m128 MinComp3(const __m128& a);
|
||||
static __m128 MinComp4(const __m128& a);
|
||||
|
||||
static __m128 MaxComp3(const __m128& a);
|
||||
static __m128 MaxComp4(const __m128& a);
|
||||
|
||||
public:
|
||||
// element access
|
||||
const float& operator[](int i) const;
|
||||
float& operator[](int i);
|
||||
|
||||
// get/set elements
|
||||
const Scalar GetX() const;
|
||||
const Scalar GetY() const;
|
||||
const Scalar GetZ() const;
|
||||
const Scalar GetW() const;
|
||||
const Scalar Get(int i) const;
|
||||
|
||||
void SetX(const Scalar& s);
|
||||
void SetY(const Scalar& s);
|
||||
void SetZ(const Scalar& s);
|
||||
void SetW(const Scalar& s);
|
||||
void Set(int i, const Scalar& s);
|
||||
|
||||
// unaligned load/store
|
||||
void LoadUnaligned3(const float* p);
|
||||
void LoadUnaligned4(const float* p);
|
||||
|
||||
void StoreUnaligned3(float* p) const;
|
||||
void StoreUnaligned4(float* p) const;
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector2Base
|
||||
class __declspec(align(8)) Vector2Base
|
||||
{
|
||||
public:
|
||||
float x, y;
|
||||
};
|
||||
|
||||
|
||||
#include "VectorBase.inl"
|
||||
|
||||
#endif //BULLET_VECTOR_BASE_H
|
||||
213
Extras/obsolete/SATConvexCollision/VectorBase.inl
Normal file
213
Extras/obsolete/SATConvexCollision/VectorBase.inl
Normal file
@@ -0,0 +1,213 @@
|
||||
// Bullet Continuous Collision Detection and Physics Library
|
||||
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
//
|
||||
//
|
||||
// VectorBase.inl
|
||||
//
|
||||
// Copyright (c) 2006 Simon Hobbs
|
||||
//
|
||||
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
//
|
||||
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
||||
//
|
||||
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
//
|
||||
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
//
|
||||
// 3. This notice may not be removed or altered from any source distribution.
|
||||
#pragma once
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Vector4Base
|
||||
|
||||
inline Vector4Base::Vector4Base()
|
||||
{
|
||||
}
|
||||
|
||||
inline const float& Vector4Base::operator[](int i) const
|
||||
{
|
||||
return *(((float*)&base) + i);
|
||||
}
|
||||
|
||||
inline float& Vector4Base::operator[](int i)
|
||||
{
|
||||
return *(((float*)&base) + i);
|
||||
}
|
||||
|
||||
inline const Scalar Vector4Base::GetX() const
|
||||
{
|
||||
return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 0, 0, 0)));
|
||||
}
|
||||
|
||||
inline const Scalar Vector4Base::GetY() const
|
||||
{
|
||||
return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(1, 1, 1, 1)));
|
||||
}
|
||||
|
||||
inline const Scalar Vector4Base::GetZ() const
|
||||
{
|
||||
return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(2, 2, 2, 2)));
|
||||
}
|
||||
|
||||
inline const Scalar Vector4Base::GetW() const
|
||||
{
|
||||
return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
}
|
||||
|
||||
inline const Scalar Vector4Base::Get(int i) const
|
||||
{
|
||||
__m128 res;
|
||||
|
||||
switch (i)
|
||||
{
|
||||
case 0: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 0, 0, 0)); break;
|
||||
case 1: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(1, 1, 1, 1)); break;
|
||||
case 2: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(2, 2, 2, 2)); break;
|
||||
case 3: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(3, 3, 3, 3)); break;
|
||||
}
|
||||
|
||||
return Scalar(res);
|
||||
}
|
||||
|
||||
inline void Vector4Base::SetX(const Scalar& s)
|
||||
{
|
||||
__m128 xxyy = _mm_shuffle_ps(s.base, base, _MM_SHUFFLE(1, 1, 0, 0));
|
||||
base = _mm_shuffle_ps(xxyy, base, _MM_SHUFFLE(3, 2, 2, 0));
|
||||
}
|
||||
|
||||
inline void Vector4Base::SetY(const Scalar& s)
|
||||
{
|
||||
__m128 xxyy = _mm_shuffle_ps(base, s.base, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
base = _mm_shuffle_ps(xxyy, base, _MM_SHUFFLE(3, 2, 2, 0));
|
||||
}
|
||||
|
||||
inline void Vector4Base::SetZ(const Scalar& s)
|
||||
{
|
||||
__m128 zzww = _mm_shuffle_ps(s.base, base, _MM_SHUFFLE(3, 3, 0, 0));
|
||||
base = _mm_shuffle_ps(base, zzww, _MM_SHUFFLE(2, 0, 1, 0));
|
||||
}
|
||||
|
||||
inline void Vector4Base::SetW(const Scalar& s)
|
||||
{
|
||||
__m128 zzww = _mm_shuffle_ps(base, s.base, _MM_SHUFFLE(0, 0, 2, 2));
|
||||
base = _mm_shuffle_ps(base, zzww, _MM_SHUFFLE(2, 0, 1, 0));
|
||||
}
|
||||
|
||||
inline void Vector4Base::Set(int i, const Scalar& s)
|
||||
{
|
||||
switch (i)
|
||||
{
|
||||
case 0: SetX(s); break;
|
||||
case 1: SetY(s); break;
|
||||
case 2: SetZ(s); break;
|
||||
case 3: SetW(s); break;
|
||||
}
|
||||
}
|
||||
|
||||
inline void Vector4Base::LoadUnaligned3(const float* p)
|
||||
{
|
||||
int* dst = (int*)this;
|
||||
dst[0] = ((const int*)p)[0];
|
||||
dst[1] = ((const int*)p)[1];
|
||||
dst[2] = ((const int*)p)[2];
|
||||
}
|
||||
|
||||
inline void Vector4Base::LoadUnaligned4(const float* p)
|
||||
{
|
||||
base = _mm_loadu_ps(p);
|
||||
}
|
||||
|
||||
inline void Vector4Base::StoreUnaligned3(float* p) const
|
||||
{
|
||||
const int* src = (const int*)this;
|
||||
((int*)p)[0] = src[0];
|
||||
((int*)p)[1] = src[1];
|
||||
((int*)p)[2] = src[2];
|
||||
}
|
||||
|
||||
inline void Vector4Base::StoreUnaligned4(float* p) const
|
||||
{
|
||||
_mm_storeu_ps(p, base);
|
||||
}
|
||||
|
||||
__forceinline void Vector4Base::Set(const __m128& x, const __m128& y, const __m128& z, const __m128& w)
|
||||
{
|
||||
__m128 xy = _mm_unpacklo_ps(x, y);
|
||||
__m128 zw = _mm_unpacklo_ps(z, w);
|
||||
base = _mm_shuffle_ps(xy, zw, _MM_SHUFFLE(1, 0, 1, 0));
|
||||
}
|
||||
|
||||
__forceinline void Vector4Base::Set(const __m128& xyz, const __m128& w)
|
||||
{
|
||||
base = _mm_shuffle_ps(xyz, xyz, _MM_SHUFFLE(0, 1, 2, 3));
|
||||
base = _mm_move_ss(base, w);
|
||||
base = _mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 1, 2, 3));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::Dot3(const __m128& v0, const __m128& v1)
|
||||
{
|
||||
__m128 a = _mm_mul_ps(v0, v1);
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
|
||||
return _mm_add_ps(b, _mm_add_ps(c, d));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::Dot4(const __m128& v0, const __m128& v1)
|
||||
{
|
||||
__m128 a = _mm_mul_ps(v0, v1);
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
|
||||
return _mm_add_ps(a, _mm_add_ps(b, _mm_add_ps(c, d)));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::Sum3(const __m128& a)
|
||||
{
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
|
||||
return _mm_add_ps(b, _mm_add_ps(c, d));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::Sum4(const __m128& a)
|
||||
{
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
|
||||
return _mm_add_ps(a, _mm_add_ps(b, _mm_add_ps(c, d)));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::MinComp3(const __m128& a)
|
||||
{
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
|
||||
return _mm_min_ps(b, _mm_min_ps(c, d));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::MinComp4(const __m128& a)
|
||||
{
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
|
||||
return _mm_min_ps(a, _mm_min_ps(b, _mm_min_ps(c, d)));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::MaxComp3(const __m128& a)
|
||||
{
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
|
||||
return _mm_max_ps(b, _mm_max_ps(c, d));
|
||||
}
|
||||
|
||||
__forceinline __m128 Vector4Base::MaxComp4(const __m128& a)
|
||||
{
|
||||
__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
|
||||
__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
|
||||
__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
|
||||
return _mm_max_ps(a, _mm_max_ps(b, _mm_max_ps(c, d)));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user