ZENO----vec.h
#pragma once
#include
#include
#include
#include
#include
namespace zeno {
namespace _impl_vec {
template
constexpr bool is_decay_same_v = std::is_same_v, std::decay_t>;
/* main class definition */
template
struct vec : std::array {
vec() = default;
explicit vec(T const &x) {
for (size_t i = 0; i < N; i++) {
(*this)[i] = x;
}
}
vec(vec &&) = default;
vec(vec const &) = default;
vec &operator=(vec const &) = default;
vec(std::array const &a) {
for (size_t i = 0; i < N; i++) {
(*this)[i] = a[i];
}
}
operator std::array() const {
std::array res;
for (size_t i = 0; i < N; i++) {
res[i] = (*this)[i];
}
return res;
}
template
explicit vec(vec const &x) {
for (size_t i = 0; i < N; i++) {
(*this)[i] = T(x[i]);
}
}
vec(std::initializer_list const &x) {
T val{};
auto it = x.begin();
for (size_t i = 0; i < N; i++) {
if (it != x.end())
val = *it++;
(*this)[i] = val;
}
}
vec(T const &x, T const &y) : vec{x, y} {}
vec(T const &x, T const &y, T const &z) : vec{x, y, z} {}
vec(T const &x, T const &y, T const &z, T const &w) : vec{x, y, z, w} {}
template
operator vec() const {
vec res;
for (size_t i = 0; i < N; i++) {
res[i] = (*this)[i];
}
return res;
}
};
/* type traits */
template
struct is_vec : std::false_type {
static constexpr size_t _N = 1;
};
template
struct is_vec> : std::true_type {
static constexpr size_t _N = N;
};
template
inline constexpr bool is_vec_v = is_vec>::value;
template
inline constexpr size_t is_vec_n = is_vec>::_N;
template
struct is_vec_promotable : std::false_type {
using type = void;
};
template
struct is_vec_promotable, vec> : std::true_type {
using type = vec;
};
template
struct is_vec_promotable, T> : std::true_type {
using type = vec;
};
template
struct is_vec_promotable> : std::true_type {
using type = vec;
};
template
struct is_vec_promotable : std::true_type {
using type = T;
};
template
inline constexpr bool is_vec_promotable_v =
is_vec_promotable, std::decay_t>::value;
template
using is_vec_promotable_t =
typename is_vec_promotable, std::decay_t>::type;
template
struct is_vec_castable : std::false_type {};
template
struct is_vec_castable, T> : std::true_type {};
template
struct is_vec_castable> : std::false_type {};
template
struct is_vec_castable : std::true_type {};
template
inline constexpr bool is_vec_castable_v =
is_vec_castable, std::decay_t>::value;
template
struct decay_vec { using type = T; };
template
struct decay_vec> { using type = T; };
template
using decay_vec_t = typename decay_vec::type;
/* converter functions */
template , bool> = true>
inline auto vec_to_other(T const &a) {
return a;
}
template
inline auto vec_to_other(vec<2, T> const &a) {
return OtherT(a[0], a[1]);
}
template
inline auto vec_to_other(vec<3, T> const &a) {
return OtherT(a[0], a[1], a[2]);
}
template
inline auto vec_to_other(vec<4, T> const &a) {
return OtherT(a[0], a[1], a[2], a[3]);
}
template
inline auto vec_to_other(vec const &a) {
OtherT res;
for (size_t i = 0; i < N; i++) {
res[i] = a[i];
}
return res;
}
template
inline auto other_to_vec(OtherT const &x) {
vec> res;
for (size_t i = 0; i < N; i++) {
res[i] = x[i];
}
return res;
}
/* element-wise operations */
template
inline auto _vec_apply(F const &f, vec const &a) {
vec res;
for (size_t i = 0; i < N; i++) {
res[i] = f(a[i]);
}
return res;
}
template , bool> = true>
inline auto _vec_apply(F const &f, T const &a) {
return f(a);
}
template
inline auto _vec_apply(F const &f, vec const &a, vec const &b) {
vec res;
for (size_t i = 0; i < N; i++) {
res[i] = f(a[i], b[i]);
}
return res;
}
template , bool> = true>
inline auto _vec_apply(F const &f, T const &a, vec const &b) {
vec res;
for (size_t i = 0; i < N; i++) {
res[i] = f(a, b[i]);
}
return res;
}
template , bool> = true>
inline auto _vec_apply(F const &f, vec const &a, S const &b) {
vec res;
for (size_t i = 0; i < N; i++) {
res[i] = f(a[i], b);
}
return res;
}
template && !is_vec_v, bool> = true>
inline auto _vec_apply(F const &f, T const &a, S const &b) {
return f(a, b);
}
template
inline constexpr bool
is__vec_apply_v = (is_vec_v || is_vec_v) &&
(std::is_arithmetic_v || std::is_arithmetic_v
|| is_vec_n == is_vec_n);
template , bool> = true>
inline auto _vec_apply(F const &f, T const &a, S const &b) {
return f(a, b);
}
#define _PER_OP2(op) \
template , bool> = true, \
decltype(std::declval>() \
op std::declval>(), \
true) = true> \
inline auto operator op(T const &a, S const &b)->decltype(auto) { \
return _vec_apply([](auto const &x, auto const &y) { return x op y; }, a, b); \
}
#define _PER_IOP2(op) \
_PER_OP2(op) \
template , S>, bool> = true, \
decltype(std::declval>() op std::declval(), true) = \
true> \
inline vec &operator op##=(vec &a, S const &b) { \
a = a op b; \
return a; \
}
_PER_IOP2(+)
_PER_IOP2(-)
_PER_IOP2(*)
_PER_IOP2(/)
_PER_IOP2(%)
_PER_IOP2(&)
_PER_IOP2(|)
_PER_IOP2(^)
_PER_IOP2(>>)
_PER_IOP2(<<)
_PER_OP2(==)
_PER_OP2(!=)
_PER_OP2(<)
_PER_OP2(>)
_PER_OP2(<=)
_PER_OP2(>=)
_PER_OP2(&&)
_PER_OP2(||)
#undef _PER_IOP2
#undef _PER_OP2
#define _PER_OP1(op) \
template , bool> = true, \
decltype(op std::declval>(), true) = true> \
inline auto operator op(T const &a) { \
return _vec_apply([](auto const &x) { return op x; }, a); \
}
_PER_OP1(+)
_PER_OP1(-)
_PER_OP1(~)
_PER_OP1(!)
#undef _PER_OP1
#define _PER_FN2(func) \
template () + std::declval(), true) = true> \
inline auto func(T const &a, S const &b)->decltype(auto) { \
return _vec_apply( \
[](auto const &x, auto const &y) { \
using promoted = decltype(x + y); \
return (promoted)std::func((promoted)x, (promoted)y); \
}, \
a, b); \
}
_PER_FN2(atan2)
_PER_FN2(pow)
_PER_FN2(max)
_PER_FN2(min)
_PER_FN2(fmod)
#undef _PER_FN2
#define _PER_FN1(func) \
template inline auto func(T const &a) { \
return _vec_apply([](auto const &x) { return (decltype(x))std::func(x); }, a); \
}
_PER_FN1(abs)
_PER_FN1(sqrt)
_PER_FN1(sin)
_PER_FN1(cos)
_PER_FN1(tan)
_PER_FN1(asin)
_PER_FN1(acos)
_PER_FN1(atan)
_PER_FN1(exp)
_PER_FN1(log)
_PER_FN1(floor)
_PER_FN1(ceil)
#undef _PER_FN1
template
inline auto fract(T const &a) {
return a - floor(a);
}
template
inline auto ifloor(T const &a) {
return toint(floor(a));
}
template
inline auto cast(T const &a) {
return _vec_apply([](auto const &x) { return (To)x; }, a);
}
template
inline auto toint(T const &a) {
return cast(a);
}
template
inline auto tofloat(T const &a) {
return cast(a);
}
/* vector math functions */
template
inline bool anytrue(vec const &a) {
bool ret = false;
for (size_t i = 0; i < N; i++) {
ret = ret || (bool)a[i];
}
return ret;
}
template
inline bool anytrue(T const &a) {
return (bool)a;
}
template
inline bool alltrue(vec const &a) {
bool ret = true;
for (size_t i = 0; i < N; i++) {
ret = ret && (bool)a[i];
}
return ret;
}
template
inline bool alltrue(T const &a) {
return (bool)a;
}
inline auto dot(float a, float b) { return a * b; }
template
inline auto dot(vec const &a, vec const &b) {
std::decay_t res(0);
for (size_t i = 0; i < N; i++) {
res += a[i] * b[i];
}
return res;
}
template
inline auto lengthSquared(vec const &a) {
std::decay_t res(0);
for (size_t i = 0; i < N; i++) {
res += a[i] * a[i];
}
return res;
}
template
inline auto length(vec const &a) {
std::decay_t res(0);
for (size_t i = 0; i < N; i++) {
res += a[i] * a[i];
}
return sqrt(res);
}
template
inline auto distance(vec const &a, vec const &b) {
return length(b - a);
}
template
inline auto normalize(vec const &a) {
return a * (1 / length(a));
}
template
inline auto normalizeSafe(vec const &a, T b = std::numeric_limits::epsilon()) {
return a * (1 / max(b, length(a)));
}
template
inline auto cross(vec<2, T> const &a, vec<2, S> const &b) {
return a[0] * b[1] - b[0] * a[1];
}
template
inline auto cross(vec<3, T> const &a, vec<3, S> const &b) {
return vec<3, decltype(a[0] * b[0])>(a[1] * b[2] - b[1] * a[2],
a[2] * b[0] - b[2] * a[0],
a[0] * b[1] - b[0] * a[1]);
}
/* generic helper functions */
template
inline auto mix(T const &a, S const &b, F const &f) {
return a * (1 - f) + b * f;
}
template
inline auto unmix(T const &a, S const &b, F const &f) {
return (f - a) / (b - a);
}
template
inline auto clamp(T const &x, S const &a, F const &b) {
return min(max(x, a), b);
}
template
inline auto minmax(T const &a, S const &b) {
return std::make_pair(min(a, b), max(a, b));
}
template , bool> = true>
inline auto tovec(T const &x) {
return vec(x);
}
template
inline auto tovec(vec const &x) { return x; }
/* common type definitions */
using vec2f = vec<2, float>;
using vec2d = vec<2, double>;
using vec2i = vec<2, int32_t>;
using vec2l = vec<2, intptr_t>;
using vec2h = vec<2, int16_t>;
using vec2c = vec<2, int8_t>;
using vec2b = vec<2, bool>;
using vec2I = vec<2, uint32_t>;
using vec2L = vec<2, uintptr_t>;
using vec2Q = vec<2, uint64_t>;
using vec2H = vec<2, uint16_t>;
using vec2C = vec<2, uint8_t>;
using vec3f = vec<3, float>;
using vec3d = vec<3, double>;
using vec3i = vec<3, int32_t>;
using vec3l = vec<3, intptr_t>;
using vec3h = vec<3, int16_t>;
using vec3c = vec<3, int8_t>;
using vec3b = vec<3, bool>;
using vec3I = vec<3, uint32_t>;
using vec3L = vec<3, uintptr_t>;
using vec3Q = vec<3, uint64_t>;
using vec3H = vec<3, uint16_t>;
using vec3C = vec<3, uint8_t>;
using vec4f = vec<4, float>;
using vec4d = vec<4, double>;
using vec4i = vec<4, int32_t>;
using vec4l = vec<4, intptr_t>;
using vec4h = vec<4, int16_t>;
using vec4c = vec<4, int8_t>;
using vec4b = vec<4, bool>;
using vec4I = vec<4, uint32_t>;
using vec4L = vec<4, uintptr_t>;
using vec4Q = vec<4, uint64_t>;
using vec4H = vec<4, uint16_t>;
using vec4C = vec<4, uint8_t>;
}
using namespace _impl_vec;
}
/* specialization for structual-binding */
namespace std {
template
struct tuple_size<::zeno::vec> : integral_constant {
};
template
struct tuple_element> {
using type = enable_if_t<(I < N), T>;
};
template
T const &get(::zeno::vec const &t) {
return t[I];
}
template
T &get(::zeno::vec &t) {
return t[I];
}
template
T &&get(::zeno::vec &&t) {
return std::move(t[I]);
}
}
game-physics-cookbook----Geometry3D.cpp
#include "Geometry3D.h"
#include
#include
#include
#define CMP(x, y) \
(fabsf(x - y) <= FLT_EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))))
float Length(const Line& line) {
return Magnitude(line.start - line.end);
}
float LengthSq(const Line& line) {
return MagnitudeSq(line.start - line.end);
}
Ray FromPoints(const Point& from, const Point& to) {
return Ray(
from,
Normalized(to - from)
);
}
vec3 GetMin(const AABB& aabb) {
vec3 p1 = aabb.position + aabb.size;
vec3 p2 = aabb.position - aabb.size;
return vec3(fminf(p1.x, p2.x), fminf(p1.y, p2.y), fminf(p1.z, p2.z));
}
vec3 GetMax(const AABB& aabb) {
vec3 p1 = aabb.position + aabb.size;
vec3 p2 = aabb.position - aabb.size;
return vec3(fmaxf(p1.x, p2.x), fmaxf(p1.y, p2.y), fmaxf(p1.z, p2.z));
}
AABB FromMinMax(const vec3& min, const vec3& max) {
return AABB((min + max) * 0.5f, (max - min) * 0.5f);
}
float PlaneEquation(const Point& point, const Plane& plane) {
return Dot(point, plane.normal) - plane.distance;
}
float PlaneEquation(const Plane& plane, const Point& point) {
return Dot(point, plane.normal) - plane.distance;
}
std::ostream& operator<<(std::ostream& os, const Line& shape) {
os << "start: (" << shape.start.x << ", " << shape.start.y << ", " << shape.start.z << "), end: (";
os << shape.end.x << ", " << shape.end.y << ", " << shape.end.z << ")";
return os;
}
std::ostream& operator<<(std::ostream& os, const Ray& shape) {
os << "origin: (" << shape.origin.x << ", " << shape.origin.y << ", " << shape.origin.z << "), ";
os << "direction: (" << shape.direction.x << ", " << shape.direction.y << ", " << shape.direction.z << ")";
return os;
}
std::ostream& operator<<(std::ostream& os, const Sphere& shape) {
os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
os << "radius: " << shape.radius;
return os;
}
std::ostream& operator<<(std::ostream& os, const AABB& shape) {
vec3 min = GetMin(shape);
vec3 max = GetMax(shape);
os << "min: (" << min.x << ", " << min.y << ", " << min.z << "), ";
os << "max: (" << max.x << ", " << max.y << ", " << max.z << ")";
return os;
}
std::ostream& operator<<(std::ostream& os, const Plane& shape) {
os << "normal: (" << shape.normal.x << ", " << shape.normal.y << ", " << shape.normal.z << "), ";
os << "distance: " << shape.distance;
return os;
}
std::ostream& operator<<(std::ostream& os, const Triangle& shape) {
os << "a: (" << shape.a.x << ", " << shape.a.y << ", " << shape.a.z << "), ";
os << "b: (" << shape.b.x << ", " << shape.b.y << ", " << shape.b.z << "), ";
os << "c: (" << shape.c.x << ", " << shape.c.y << ", " << shape.c.z << ")";
return os;
}
std::ostream& operator<<(std::ostream& os, const OBB& shape) {
os << "position:" << shape.position.x << ", " << shape.position.y << ", " << shape.position.z << "), ";
os << "size:" << shape.size.x << ", " << shape.size.y << ", " << shape.size.z << "), ";
os << "x basis:" << shape.orientation._11 << ", " << shape.orientation._21 << ", " << shape.orientation._31 << "), ";
os << "y basis:" << shape.orientation._12 << ", " << shape.orientation._22 << ", " << shape.orientation._32 << "), ";
os << "z basis:" << shape.orientation._13 << ", " << shape.orientation._23 << ", " << shape.orientation._33 << ")";
return os;
}
bool PointInSphere(const Point& point, const Sphere& sphere) {
return MagnitudeSq(point - sphere.position) < sphere.radius * sphere.radius;
}
bool PointOnPlane(const Point& point, const Plane& plane) {
// This should probably use an epsilon!
//return Dot(point, plane.normal) - plane.distance == 0.0f;
return CMP(Dot(point, plane.normal) - plane.distance, 0.0f);
}
bool PointInAABB(const Point& point, const AABB& aabb) {
Point min = GetMin(aabb);
Point max = GetMax(aabb);
if (point.x < min.x || point.y < min.y || point.z < min.z) {
return false;
}
if (point.x > max.x || point.y > max.y || point.z > max.z) {
return false;
}
return true;
}
bool PointInOBB(const Point& point, const OBB& obb) {
vec3 dir = point - obb.position;
for (int i = 0; i < 3; ++i) {
const float* orientation = &obb.orientation.asArray[i * 3];
vec3 axis(orientation[0], orientation[1], orientation[2]);
float distance = Dot(dir, axis);
if (distance > obb.size.asArray[i]) {
return false;
}
if (distance < -obb.size.asArray[i]) {
return false;
}
}
return true;
}
Point ClosestPoint(const Sphere& sphere, const Point& point) {
vec3 sphereToPoint = point - sphere.position;
Normalize(sphereToPoint);
sphereToPoint = sphereToPoint * sphere.radius;
return sphereToPoint + sphere.position;
}
Point ClosestPoint(const AABB& aabb, const Point& point) {
Point result = point;
Point min = GetMin(aabb);
Point max = GetMax(aabb);
result.x = (result.x < min.x) ? min.x : result.x;
result.y = (result.y < min.x) ? min.y : result.y;
result.z = (result.z < min.x) ? min.z : result.z;
result.x = (result.x > max.x) ? max.x : result.x;
result.y = (result.y > max.x) ? max.y : result.y;
result.z = (result.z > max.x) ? max.z : result.z;
return result;
}
Point ClosestPoint(const OBB& obb, const Point& point) {
Point result = obb.position;
vec3 dir = point - obb.position;
for (int i = 0; i < 3; ++i) {
const float* orientation = &obb.orientation.asArray[i * 3];
vec3 axis(orientation[0], orientation[1], orientation[2]);
float distance = Dot(dir, axis);
if (distance > obb.size.asArray[i]) {
distance = obb.size.asArray[i];
}
if (distance < -obb.size.asArray[i]) {
distance = -obb.size.asArray[i];
}
result = result + (axis * distance);
}
return result;
}
Point ClosestPoint(const Plane& plane, const Point& point) {
// This works assuming plane.Normal is normalized, which it should be
float distance = Dot(plane.normal, point) - plane.distance;
// If the plane normal wasn't normalized, we'd need this:
// distance = distance / DOT(plane.Normal, plane.Normal);
return point - plane.normal * distance;
}
bool PointOnLine(const Point& point, const Line& line) {
Point closest = ClosestPoint(line, point);
float distanceSq = MagnitudeSq(closest - point);
return CMP(distanceSq, 0.0f);
}
Point ClosestPoint(const Line& line, const Point& point) {
vec3 lVec = line.end - line.start; // Line Vector
// Project "point" onto the "Line Vector", computing:
// closest(t) = start + t * (end - start)
// T is how far along the line the projected point is
float t = Dot(point - line.start, lVec) / Dot(lVec, lVec);
// Clamp t to the 0 to 1 range
t = fmaxf(t, 0.0f);
t = fminf(t, 1.0f);
// Return projected position of t
return line.start + lVec * t;
}
bool PointOnRay(const Point& point, const Ray& ray) {
if (point == ray.origin) {
return true;
}
vec3 norm = point - ray.origin;
Normalize(norm);
float diff = Dot(norm, ray.direction); // Direction is normalized
// If BOTH vectors point in the same direction, diff should be 1
return CMP(diff, 1.0f);
}
Point ClosestPoint(const Ray& ray, const Point& point) {
// Project point onto ray,
float t = Dot(point - ray.origin, ray.direction);
// Not needed if direction is normalized!
// t /= Dot(ray.direction, ray.direction);
// We only want to clamp t in the positive direction.
// The ray extends infinatley in this direction!
t = fmaxf(t, 0.0f);
// Compute the projected position from the clamped t
// Notice we multiply r.Normal by t, not AB.
// This is becuase we want the ray in the direction
// of the normal, which technically the line segment is
// but this is much more explicit and easy to read.
return Point(ray.origin + ray.direction * t);
}
bool PointInPlane(const Point& point, const Plane& plane) {
return PointOnPlane(point, plane);
}
bool PointInLine(const Point& point, const Line& line) {
return PointOnLine(point, line);
}
bool PointInRay(const Point& point, const Ray& ray) {
return PointOnRay(point, ray);
}
bool ContainsPoint(const Sphere& sphere, const Point& point) {
return PointInSphere(point, sphere);
}
bool ContainsPoint(const Point& point, const Sphere& sphere) {
return PointInSphere(point, sphere);
}
bool ContainsPoint(const AABB& aabb, const Point& point) {
return PointInAABB(point, aabb);
}
bool ContainsPoint(const Point& point, const AABB& aabb) {
return PointInAABB(point, aabb);
}
bool ContainsPoint(const Point& point, const OBB& obb) {
return PointInOBB(point, obb);
}
bool ContainsPoint(const OBB& obb, const Point& point) {
return PointInOBB(point, obb);
}
bool ContainsPoint(const Point& point, const Plane& plane) {
return PointOnPlane(point, plane);
}
bool ContainsPoint(const Plane& plane, const Point& point) {
return PointOnPlane(point, plane);
}
bool ContainsPoint(const Point& point, const Line& line) {
return PointOnLine(point, line);
}
bool ContainsPoint(const Line& line, const Point& point) {
return PointOnLine(point, line);
}
bool ContainsPoint(const Point& point, const Ray& ray) {
return PointOnRay(point, ray);
}
bool ContainsPoint(const Ray& ray, const Point& point) {
return PointOnRay(point, ray);
}
Point ClosestPoint(const Point& point, const Sphere& sphere) {
return ClosestPoint(sphere, point);
}
Point ClosestPoint(const Point& point, const AABB& aabb) {
return ClosestPoint(aabb, point);
}
Point ClosestPoint(const Point& point, const OBB& obb) {
return ClosestPoint(obb, point);
}
Point ClosestPoint(const Point& point, const Plane& plane) {
return ClosestPoint(plane, point);
}
Point ClosestPoint(const Point& point, const Line& line) {
return ClosestPoint(line, point);
}
Point ClosestPoint(const Point& point, const Ray& ray) {
return ClosestPoint(ray, point);
}
Point ClosestPoint(const Point& p, const Triangle& t) {
return ClosestPoint(t, p);
}
bool SphereSphere(const Sphere& s1, const Sphere& s2) {
float radiiSum = s1.radius + s2.radius;
float sqDistance = MagnitudeSq(s1.position - s2.position);
return sqDistance < radiiSum * radiiSum;
}
bool SphereAABB(const Sphere& sphere, const AABB& aabb) {
Point closestPoint = ClosestPoint(aabb, sphere.position);
float distSq = MagnitudeSq(sphere.position - closestPoint);
float radiusSq = sphere.radius * sphere.radius;
return distSq < radiusSq;
}
bool SphereOBB(const Sphere& sphere, const OBB& obb) {
Point closestPoint = ClosestPoint(obb, sphere.position);
float distSq = MagnitudeSq(sphere.position - closestPoint);
float radiusSq = sphere.radius * sphere.radius;
return distSq < radiusSq;
}
bool SpherePlane(const Sphere& sphere, const Plane& plane) {
Point closestPoint = ClosestPoint(plane, sphere.position);
float distSq = MagnitudeSq(sphere.position - closestPoint);
float radiusSq = sphere.radius * sphere.radius;
return distSq < radiusSq;
}
bool AABBAABB(const AABB& aabb1, const AABB& aabb2) {
Point aMin = GetMin(aabb1);
Point aMax = GetMax(aabb1);
Point bMin = GetMin(aabb2);
Point bMax = GetMax(aabb2);
return (aMin.x <= bMax.x && aMax.x >= bMin.x) &&
(aMin.y <= bMax.y && aMax.y >= bMin.y) &&
(aMin.z <= bMax.z && aMax.z >= bMin.z);
}
bool AABBOBB(const AABB& aabb, const OBB& obb) {
const float* o = obb.orientation.asArray;
vec3 test[15] = {
vec3(1, 0, 0), // AABB axis 1
vec3(0, 1, 0), // AABB axis 2
vec3(0, 0, 1), // AABB axis 3
vec3(o[0], o[1], o[2]),
vec3(o[3], o[4], o[5]),
vec3(o[6], o[7], o[8])
};
for (int i = 0; i < 3; ++i) { // Fill out rest of axis
test[6 + i * 3 + 0] = Cross(test[i], test[0]);
test[6 + i * 3 + 1] = Cross(test[i], test[1]);
test[6 + i * 3 + 2] = Cross(test[i], test[2]);
}
for (int i = 0; i < 15; ++i) {
if (!OverlapOnAxis(aabb, obb, test[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool OverlapOnAxis(const AABB& aabb, const OBB& obb, const vec3& axis) {
Interval a = GetInterval(aabb, axis);
Interval b = GetInterval(obb, axis);
return ((b.min <= a.max) && (a.min <= b.max));
}
bool OverlapOnAxis(const OBB& obb1, const OBB& obb2, const vec3& axis) {
Interval a = GetInterval(obb1, axis);
Interval b = GetInterval(obb1, axis);
return ((b.min <= a.max) && (a.min <= b.max));
}
bool OverlapOnAxis(const AABB& aabb, const Triangle& triangle, const vec3& axis) {
Interval a = GetInterval(aabb, axis);
Interval b = GetInterval(triangle, axis);
return ((b.min <= a.max) && (a.min <= b.max));
}
bool OverlapOnAxis(const OBB& obb, const Triangle& triangle, const vec3& axis) {
Interval a = GetInterval(obb, axis);
Interval b = GetInterval(triangle, axis);
return ((b.min <= a.max) && (a.min <= b.max));
}
bool OverlapOnAxis(const Triangle& t1, const Triangle& t2, const vec3& axis) {
Interval a = GetInterval(t1, axis);
Interval b = GetInterval(t2, axis);
return ((b.min <= a.max) && (a.min <= b.max));
}
Interval GetInterval(const Triangle& triangle, const vec3& axis) {
Interval result;
result.min = Dot(axis, triangle.points[0]);
result.max = result.min;
for (int i = 1; i < 3; ++i) {
float value = Dot(axis, triangle.points[i]);
result.min = fminf(result.min, value);
result.max = fmaxf(result.max, value);
}
return result;
}
Interval GetInterval(const OBB& obb, const vec3& axis) {
vec3 vertex[8];
vec3 C = obb.position; // OBB Center
vec3 E = obb.size; // OBB Extents
const float* o = obb.orientation.asArray;
vec3 A[] = { // OBB Axis
vec3(o[0], o[1], o[2]),
vec3(o[3], o[4], o[5]),
vec3(o[6], o[7], o[8]),
};
vertex[0] = C + A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
vertex[1] = C - A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
vertex[2] = C + A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
vertex[3] = C + A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
vertex[4] = C - A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
vertex[5] = C + A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
vertex[6] = C - A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
vertex[7] = C - A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
Interval result;
result.min = result.max = Dot(axis, vertex[0]);
for (int i = 1; i < 8; ++i) {
float projection = Dot(axis, vertex[i]);
result.min = (projection < result.min) ? projection : result.min;
result.max = (projection > result.max) ? projection : result.max;
}
return result;
}
Interval GetInterval(const AABB& aabb, const vec3& axis) {
vec3 i = GetMin(aabb);
vec3 a = GetMax(aabb);
vec3 vertex[8] = {
vec3(i.x, a.y, a.z),
vec3(i.x, a.y, i.z),
vec3(i.x, i.y, a.z),
vec3(i.x, i.y, i.z),
vec3(a.x, a.y, a.z),
vec3(a.x, a.y, i.z),
vec3(a.x, i.y, a.z),
vec3(a.x, i.y, i.z)
};
Interval result;
result.min = result.max = Dot(axis, vertex[0]);
for (int i = 1; i < 8; ++i) {
float projection = Dot(axis, vertex[i]);
result.min = (projection < result.min) ? projection : result.min;
result.max = (projection > result.max) ? projection : result.max;
}
return result;
}
bool AABBPlane(const AABB& aabb, const Plane& plane) {
// Project the half extents of the AABB onto the plane normal
float pLen =aabb.size.x * fabsf(plane.normal.x) +
aabb.size.y * fabsf(plane.normal.y) +
aabb.size.z * fabsf(plane.normal.z);
// Find the distance from the center of the AABB to the plane
float dist = Dot(plane.normal, aabb.position) - plane.distance;
// Intersection occurs if the distance falls within the projected side
return fabsf(dist) <= pLen;
}
bool OBBOBB(const OBB& obb1, const OBB& obb2) {
const float* o1 = obb1.orientation.asArray;
const float* o2 = obb2.orientation.asArray;
vec3 test[15] = {
vec3(o1[0], o1[1], o1[2]),
vec3(o1[3], o1[4], o1[5]),
vec3(o1[6], o1[7], o1[8]),
vec3(o2[0], o2[1], o2[2]),
vec3(o2[3], o2[4], o2[5]),
vec3(o2[6], o2[7], o2[8])
};
for (int i = 0; i < 3; ++i) { // Fill out rest of axis
test[6 + i * 3 + 0] = Cross(test[i], test[0]);
test[6 + i * 3 + 1] = Cross(test[i], test[1]);
test[6 + i * 3 + 2] = Cross(test[i], test[2]);
}
for (int i = 0; i < 15; ++i) {
if (!OverlapOnAxis(obb1, obb2, test[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool OBBPlane(const OBB& obb, const Plane& plane) {
// Local variables for readability only
const float* o = obb.orientation.asArray;
vec3 rot[] = { // rotation / orientation
vec3(o[0], o[1], o[2]),
vec3(o[3], o[4], o[5]),
vec3(o[6], o[7], o[8]),
};
vec3 normal = plane.normal;
// Project the half extents of the AABB onto the plane normal
float pLen =obb.size.x * fabsf(Dot(normal, rot[0])) +
obb.size.y * fabsf(Dot(normal, rot[1])) +
obb.size.z * fabsf(Dot(normal, rot[2]));
// Find the distance from the center of the OBB to the plane
float dist = Dot(plane.normal, obb.position) - plane.distance;
// Intersection occurs if the distance falls within the projected side
return fabsf(dist) <= pLen;
}
bool PlanePlane(const Plane& plane1, const Plane& plane2) {
// Compute direction of intersection line
vec3 d = Cross(plane1.normal, plane2.normal);
// Check the length of the direction line
// if the length is 0, no intersection happened
return !(CMP(Dot(d, d), 0));
// We could have used the dot product here, instead of the cross product
}
bool Raycast(const Sphere& sphere, const Ray& ray, RaycastResult* outResult) {
ResetRaycastResult(outResult);
vec3 e = sphere.position - ray.origin;
float rSq = sphere.radius * sphere.radius;
float eSq = MagnitudeSq(e);
float a = Dot(e, ray.direction); // ray.direction is assumed to be normalized
float bSq = /*sqrtf(*/eSq - (a * a)/*)*/;
float f = sqrt(fabsf((rSq)- /*(b * b)*/bSq));
// Assume normal intersection!
float t = a - f;
// No collision has happened
if (rSq - (eSq - a * a) < 0.0f) {
return false;
}
// Ray starts inside the sphere
else if (eSq < rSq) {
// Just reverse direction
t = a + f;
}
if (outResult != 0) {
outResult->t = t;
outResult->hit = true;
outResult->point = ray.origin + ray.direction * t;
outResult->normal = Normalized(outResult->point - sphere.position);
}
return true;
}
bool Raycast(const OBB& obb, const Ray& ray, RaycastResult* outResult) {
ResetRaycastResult(outResult);
const float* o = obb.orientation.asArray;
const float* size = obb.size.asArray;
vec3 p = obb.position - ray.origin;
vec3 X(o[0], o[1], o[2]);
vec3 Y(o[3], o[4], o[5]);
vec3 Z(o[6], o[7], o[8]);
vec3 f(
Dot(X, ray.direction),
Dot(Y, ray.direction),
Dot(Z, ray.direction)
);
vec3 e(
Dot(X, p),
Dot(Y, p),
Dot(Z, p)
);
#if 1
float t[6] = { 0, 0, 0, 0, 0, 0 };
for (int i = 0; i < 3; ++i) {
if (CMP(f[i], 0)) {
if (-e[i] - size[i] > 0 || -e[i] + size[i] < 0) {
return false;
}
f[i] = 0.00001f; // Avoid div by 0!
}
t[i * 2 + 0] = (e[i] + size[i]) / f[i]; // tmin[x, y, z]
t[i * 2 + 1] = (e[i] - size[i]) / f[i]; // tmax[x, y, z]
}
float tmin = fmaxf(fmaxf(fminf(t[0], t[1]), fminf(t[2], t[3])), fminf(t[4], t[5]));
float tmax = fminf(fminf(fmaxf(t[0], t[1]), fmaxf(t[2], t[3])), fmaxf(t[4], t[5]));
#else
// The above loop simplifies the below if statements
// this is done to make sure the sample fits into the book
if (CMP(f.x, 0)) {
if (-e.x - obb.size.x > 0 || -e.x + obb.size.x < 0) {
return -1;
}
f.x = 0.00001f; // Avoid div by 0!
}
else if (CMP(f.y, 0)) {
if (-e.y - obb.size.y > 0 || -e.y + obb.size.y < 0) {
return -1;
}
f.y = 0.00001f; // Avoid div by 0!
}
else if (CMP(f.z, 0)) {
if (-e.z - obb.size.z > 0 || -e.z + obb.size.z < 0) {
return -1;
}
f.z = 0.00001f; // Avoid div by 0!
}
float t1 = (e.x + obb.size.x) / f.x;
float t2 = (e.x - obb.size.x) / f.x;
float t3 = (e.y + obb.size.y) / f.y;
float t4 = (e.y - obb.size.y) / f.y;
float t5 = (e.z + obb.size.z) / f.z;
float t6 = (e.z - obb.size.z) / f.z;
float tmin = fmaxf(fmaxf(fminf(t1, t2), fminf(t3, t4)), fminf(t5, t6));
float tmax = fminf(fminf(fmaxf(t1, t2), fmaxf(t3, t4)), fmaxf(t5, t6));
#endif
// if tmax < 0, ray is intersecting AABB
// but entire AABB is behing it's origin
if (tmax < 0) {
return false;
}
// if tmin > tmax, ray doesn't intersect AABB
if (tmin > tmax) {
return false;
}
// If tmin is < 0, tmax is closer
float t_result = tmin;
if (tmin < 0.0f) {
t_result = tmax;
}
if (outResult != 0) {
outResult->hit = true;
outResult->t = t_result;
outResult->point = ray.origin + ray.direction * t_result;
vec3 normals[] = {
X, // +x
X * -1.0f, // -x
Y, // +y
Y * -1.0f, // -y
Z, // +z
Z * -1.0f // -z
};
for (int i = 0; i < 6; ++i) {
if (CMP(t_result, t[i])) {
outResult->normal = Normalized(normals[i]);
}
}
}
return true;
}
void ResetRaycastResult(RaycastResult* outResult) {
if (outResult != 0) {
outResult->t = -1;
outResult->hit = false;
outResult->normal = vec3(0, 0, 1);
outResult->point = vec3(0, 0, 0);
}
}
bool Raycast(const AABB& aabb, const Ray& ray, RaycastResult* outResult) {
ResetRaycastResult(outResult);
vec3 min = GetMin(aabb);
vec3 max = GetMax(aabb);
// Any component of direction could be 0!
// Address this by using a small number, close to
// 0 in case any of directions components are 0
float t1 = (min.x - ray.origin.x) / (CMP(ray.direction.x, 0.0f) ? 0.00001f : ray.direction.x);
float t2 = (max.x - ray.origin.x) / (CMP(ray.direction.x, 0.0f) ? 0.00001f : ray.direction.x);
float t3 = (min.y - ray.origin.y) / (CMP(ray.direction.y, 0.0f) ? 0.00001f : ray.direction.y);
float t4 = (max.y - ray.origin.y) / (CMP(ray.direction.y, 0.0f) ? 0.00001f : ray.direction.y);
float t5 = (min.z - ray.origin.z) / (CMP(ray.direction.z, 0.0f) ? 0.00001f : ray.direction.z);
float t6 = (max.z - ray.origin.z) / (CMP(ray.direction.z, 0.0f) ? 0.00001f : ray.direction.z);
float tmin = fmaxf(fmaxf(fminf(t1, t2), fminf(t3, t4)), fminf(t5, t6));
float tmax = fminf(fminf(fmaxf(t1, t2), fmaxf(t3, t4)), fmaxf(t5, t6));
// if tmax < 0, ray is intersecting AABB
// but entire AABB is behing it's origin
if (tmax < 0) {
return false;
}
// if tmin > tmax, ray doesn't intersect AABB
if (tmin > tmax) {
return false;
}
float t_result = tmin;
// If tmin is < 0, tmax is closer
if (tmin < 0.0f) {
t_result = tmax;
}
if (outResult != 0) {
outResult->t = t_result;
outResult->hit = true;
outResult->point = ray.origin + ray.direction * t_result;
vec3 normals[] = {
vec3(-1, 0, 0),
vec3(1, 0, 0),
vec3(0, -1, 0),
vec3(0, 1, 0),
vec3(0, 0, -1),
vec3(0, 0, 1)
};
float t[] = { t1, t2, t3, t4, t5, t6 };
for (int i = 0; i < 6; ++i) {
if (CMP(t_result, t[i])) {
outResult->normal = normals[i];
}
}
}
return true;
}
bool Raycast(const Plane& plane, const Ray& ray, RaycastResult* outResult) {
ResetRaycastResult(outResult);
float nd = Dot(ray.direction, plane.normal);
float pn = Dot(ray.origin, plane.normal);
// nd must be negative, and not 0
// if nd is positive, the ray and plane normals
// point in the same direction. No intersection.
if (nd >= 0.0f) {
return false;
}
float t = (plane.distance - pn) / nd;
// t must be positive
if (t >= 0.0f) {
if (outResult != 0) {
outResult->t = t;
outResult->hit = true;
outResult->point = ray.origin + ray.direction * t;
outResult->normal = Normalized(plane.normal);
}
return true;
}
return false;
}
bool Linetest(const Sphere& sphere, const Line& line) {
Point closest = ClosestPoint(line, sphere.position);
float distSq = MagnitudeSq(sphere.position - closest);
return distSq <= (sphere.radius * sphere.radius);
}
bool Linetest(const Plane& plane, const Line& line) {
vec3 ab = line.end - line.start;
float nA = Dot(plane.normal, line.start);
float nAB = Dot(plane.normal, ab);
if (CMP(nAB, 0)) {
return false;
}
float t = (plane.distance - nA) / nAB;
return t >= 0.0f && t <= 1.0f;
}
bool Linetest(const AABB& aabb, const Line& line) {
Ray ray;
ray.origin = line.start;
ray.direction = Normalized(line.end - line.start);
RaycastResult raycast;
if (!Raycast(aabb, ray, &raycast)) {
return false;
}
float t = raycast.t;
return t >= 0 && t * t <= LengthSq(line);
}
bool Linetest(const OBB& obb, const Line& line) {
if (MagnitudeSq(line.end - line.start) < 0.0000001f) {
return PointInOBB(line.start, obb);
}
Ray ray;
ray.origin = line.start;
ray.direction = Normalized(line.end - line.start);
RaycastResult result;
if (!Raycast(obb, ray, &result)) {
return false;
}
float t = result.t;
return t >= 0 && t * t <= LengthSq(line);
}
bool Raycast(const Ray& ray, const Sphere& sphere, RaycastResult* outResult) {
return Raycast(sphere, ray, outResult);
}
bool Raycast(const Ray& ray, const AABB& aabb, RaycastResult* outResult) {
return Raycast(aabb, ray, outResult);
}
bool Raycast(const Ray& ray, const OBB& obb, RaycastResult* outResult) {
return Raycast(obb, ray, outResult);
}
bool Raycast(const Ray& ray, const Plane& plane, RaycastResult* outResult) {
return Raycast(plane, ray, outResult);
}
bool Linetest(const Line& line, const Sphere& sphere) {
return Linetest(sphere, line);
}
bool Linetest(const Line& line, const AABB& aabb) {
return Linetest(aabb, line);
}
bool Linetest(const Line& line, const OBB& obb) {
return Linetest(obb, line);
}
bool Linetest(const Line& line, const Plane& plane) {
return Linetest(plane, line);
}
vec3 Centroid(const Triangle& t) {
vec3 result;
result.x = t.a.x + t.b.x + t.c.x;
result.y = t.a.y + t.b.y + t.c.y;
result.z = t.a.z + t.b.z + t.c.z;
result = result * (1.0f / 3.0f);
return result;
}
bool PointInTriangle(const Point& p, const Triangle& t) {
// Move the triangle so that the point is
// now at the origin of the triangle
vec3 a = t.a - p;
vec3 b = t.b - p;
vec3 c = t.c - p;
// The point should be moved too, so they are both
// relative, but because we don't use p in the
// equation anymore, we don't need it!
// p -= p; // This would just equal the zero vector!
vec3 normPBC = Cross(b, c); // Normal of PBC (u)
vec3 normPCA = Cross(c, a); // Normal of PCA (v)
vec3 normPAB = Cross(a, b); // Normal of PAB (w)
// Test to see if the normals are facing
// the same direction, return false if not
if (Dot(normPBC, normPCA) < 0.0f) {
return false;
}
else if (Dot(normPBC, normPAB) < 0.0f) {
return false;
}
// All normals facing the same way, return true
return true;
}
vec3 BarycentricOptimized(const Point& p, const Triangle& t) {
vec3 v0 = t.b - t.a;
vec3 v1 = t.c - t.a;
vec3 v2 = p - t.a;
float d00 = Dot(v0, v0);
float d01 = Dot(v0, v1);
float d11 = Dot(v1, v1);
float d20 = Dot(v2, v0);
float d21 = Dot(v2, v1);
float denom = d00 * d11 - d01 * d01;
if (CMP(denom, 0.0f)) {
return vec3();
}
vec3 result;
result.y = (d11 * d20 - d01 * d21) / denom;
result.z = (d00 * d21 - d01 * d20) / denom;
result.x = 1.0f - result.y - result.z;
return result;
}
vec3 Barycentric(const Point& p, const Triangle& t) {
vec3 ap = p - t.a;
vec3 bp = p - t.b;
vec3 cp = p - t.c;
vec3 ab = t.b - t.a;
vec3 ac = t.c - t.a;
vec3 bc = t.c - t.b;
vec3 cb = t.b - t.c;
vec3 ca = t.a - t.c;
vec3 v = ab - Project(ab, cb);
float a = 1.0f - (Dot(v, ap) / Dot(v, ab));
v = bc - Project(bc, ac);
float b = 1.0f - (Dot(v, bp) / Dot(v, bc));
v = ca - Project(ca, ab);
float c = 1.0f - (Dot(v, cp) / Dot(v, ca));
return vec3(a, b, c);
}
Plane FromTriangle(const Triangle& t) {
Plane result;
result.normal = Normalized(Cross(t.b - t.a, t.c - t.a));
result.distance = Dot(result.normal, t.a);
return result;
}
Point ClosestPoint(const Triangle& t, const Point& p) {
Plane plane = FromTriangle(t);
Point closest = ClosestPoint(plane, p);
// Closest point was inside triangle
if (PointInTriangle(closest, t)) {
return closest;
}
Point c1 = ClosestPoint(Line(t.a, t.b), closest); // Line AB
Point c2 = ClosestPoint(Line(t.b, t.c), closest); // Line BC
Point c3 = ClosestPoint(Line(t.c, t.a), closest); // Line CA
float magSq1 = MagnitudeSq(closest - c1);
float magSq2 = MagnitudeSq(closest - c2);
float magSq3 = MagnitudeSq(closest - c3);
if (magSq1 < magSq2 && magSq1 < magSq3) {
return c1;
}
else if (magSq2 < magSq1 && magSq2 < magSq3) {
return c2;
}
return c3;
}
bool TriangleSphere(const Triangle& t, const Sphere& s) {
Point closest = ClosestPoint(t, s.position);
float magSq = MagnitudeSq(closest - s.position);
return magSq <= s.radius * s.radius;
}
bool TriangleAABB(const Triangle& t, const AABB& a) {
// Compute the edge vectors of the triangle (ABC)
vec3 f0 = t.b - t.a;
vec3 f1 = t.c - t.b;
vec3 f2 = t.a - t.c;
// Compute the face normals of the AABB
vec3 u0(1.0f, 0.0f, 0.0f);
vec3 u1(0.0f, 1.0f, 0.0f);
vec3 u2(0.0f, 0.0f, 1.0f);
vec3 test[13] = {
// 3 Normals of AABB
u0, // AABB Axis 1
u1, // AABB Axis 2
u2, // AABB Axis 3
// 1 Normal of the Triangle
Cross(f0, f1),
// 9 Axis, cross products of all edges
Cross(u0, f0),
Cross(u0, f1),
Cross(u0, f2),
Cross(u1, f0),
Cross(u1, f1),
Cross(u1, f2),
Cross(u2, f0),
Cross(u2, f1),
Cross(u2, f2)
};
for (int i = 0; i < 13; ++i) {
if (!OverlapOnAxis(a, t, test[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool TriangleOBB(const Triangle& t, const OBB& o) {
// Compute the edge vectors of the triangle (ABC)
vec3 f0 = t.b - t.a;
vec3 f1 = t.c - t.b;
vec3 f2 = t.a - t.c;
// Compute the face normals of the AABB
const float* orientation = o.orientation.asArray;
vec3 u0(orientation[0], orientation[1], orientation[2]);
vec3 u1(orientation[3], orientation[4], orientation[5]);
vec3 u2(orientation[6], orientation[7], orientation[8]);
vec3 test[13] = {
// 3 Normals of AABB
u0, // AABB Axis 1
u1, // AABB Axis 2
u2, // AABB Axis 3
// 1 Normal of the Triangle
Cross(f0, f1),
// 9 Axis, cross products of all edges
Cross(u0, f0),
Cross(u0, f1),
Cross(u0, f2),
Cross(u1, f0),
Cross(u1, f1),
Cross(u1, f2),
Cross(u2, f0),
Cross(u2, f1),
Cross(u2, f2)
};
for (int i = 0; i < 13; ++i) {
if (!OverlapOnAxis(o, t, test[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool TriangleTriangle(const Triangle& t1, const Triangle& t2) {
#if 0
vec3 axisToTest[] = {
// Triangle 1, Normal
SatCrossEdge(t1.a, t1.b, t1.b, t1.c),
// Triangle 2, Normal
SatCrossEdge(t2.a, t2.b, t2.b, t2.c),
// Cross Product of edges
SatCrossEdge(t2.a, t2.b, t1.a, t1.b),
SatCrossEdge(t2.a, t2.b, t1.b, t1.c),
SatCrossEdge(t2.a, t2.b, t1.c, t1.a),
SatCrossEdge(t2.b, t2.c, t1.a, t1.b),
SatCrossEdge(t2.b, t2.c, t1.b, t1.c),
SatCrossEdge(t2.b, t2.c, t1.c, t1.a),
SatCrossEdge(t2.c, t2.a, t1.a, t1.b),
SatCrossEdge(t2.c, t2.a, t1.b, t1.c),
SatCrossEdge(t2.c, t2.a, t1.c, t1.a),
};
#else
vec3 t1_f0 = t1.b - t1.a; // Edge 0
vec3 t1_f1 = t1.c - t1.b; // Edge 1
vec3 t1_f2 = t1.a - t1.c; // Edge 2
vec3 t2_f0 = t2.b - t2.a; // Edge 0
vec3 t2_f1 = t2.c - t2.b; // Edge 1
vec3 t2_f2 = t2.a - t2.c; // Edge 2
vec3 axisToTest[] = {
// Triangle 1, Normal
Cross(t1_f0, t1_f1),
// Triangle 2, Normal
Cross(t2_f0, t2_f1),
// Cross Product of edges
Cross(t2_f0, t1_f0),
Cross(t2_f0, t1_f1),
Cross(t2_f0, t1_f2),
Cross(t2_f1, t1_f0),
Cross(t2_f1, t1_f1),
Cross(t2_f1, t1_f2),
Cross(t2_f2, t1_f0),
Cross(t2_f2, t1_f1),
Cross(t2_f2, t1_f2),
};
#endif
for (int i = 0; i < 11; ++i) {
if (!OverlapOnAxis(t1, t2, axisToTest[i])) {
return false; // Seperating axis found
}
}
return true; // Seperating axis not found
}
bool TriangleTriangleRobust(const Triangle& t1, const Triangle& t2) {
vec3 axisToTest[] = {
// Triangle 1, Normal
SatCrossEdge(t1.a, t1.b, t1.b, t1.c),
// Triangle 2, Normal
SatCrossEdge(t2.a, t2.b, t2.b, t2.c),
// Cross Product of edges
SatCrossEdge(t2.a, t2.b, t1.a, t1.b),
SatCrossEdge(t2.a, t2.b, t1.b, t1.c),
SatCrossEdge(t2.a, t2.b, t1.c, t1.a),
SatCrossEdge(t2.b, t2.c, t1.a, t1.b),
SatCrossEdge(t2.b, t2.c, t1.b, t1.c),
SatCrossEdge(t2.b, t2.c, t1.c, t1.a),
SatCrossEdge(t2.c, t2.a, t1.a, t1.b),
SatCrossEdge(t2.c, t2.a, t1.b, t1.c),
SatCrossEdge(t2.c, t2.a, t1.c, t1.a),
};
for (int i = 0; i < 11; ++i) {
if (!OverlapOnAxis(t1, t2, axisToTest[i])) {
if (!CMP(MagnitudeSq(axisToTest[i]), 0)) {
return false; // Seperating axis found
}
}
}
return true; // Seperating axis not found
}
vec3 SatCrossEdge(const vec3& a, const vec3& b, const vec3& c, const vec3& d) {
vec3 ab = b - a;
vec3 cd = d - c;
vec3 result = Cross(ab, cd);
if (!CMP(MagnitudeSq(result), 0)) { // Is ab and cd parallel?
return result; // Not parallel!
}
else { // ab and cd are parallel
// Get an axis perpendicular to AB
vec3 axis = Cross(ab, c - a);
result = Cross(ab, axis);
if (!CMP(MagnitudeSq(result), 0)) { // Still parallel?
return result; // Not parallel
}
}
// New axis being tested is parallel too.
// This means that a, b, c and d are on a line
// Nothing we can do!
return vec3();
}
Point debugRaycastResult;
bool Raycast(const Triangle& triangle, const Ray& ray, RaycastResult* outResult) {
ResetRaycastResult(outResult);
Plane plane = FromTriangle(triangle);
RaycastResult planeResult;
if (!Raycast(plane, ray, &planeResult)) {
return false;
}
float t = planeResult.t;
Point result = ray.origin + ray.direction * t;
vec3 barycentric = Barycentric(result, triangle);
if (barycentric.x >= 0.0f && barycentric.x <= 1.0f &&
barycentric.y >= 0.0f && barycentric.y <= 1.0f &&
barycentric.z >= 0.0f && barycentric.z <= 1.0f) {
if (outResult != 0) {
outResult->t = t;
outResult->hit = true;
outResult->point = ray.origin + ray.direction * t;
outResult->normal = plane.normal;
}
return true;
}
return false;
}
bool Linetest(const Triangle& triangle, const Line& line) {
Ray ray;
ray.origin = line.start;
ray.direction = Normalized(line.end - line.start);
RaycastResult raycast;
if (!Raycast(triangle, ray, &raycast)) {
return false;
}
float t = raycast.t;
return t >= 0 && t * t <= LengthSq(line);
}
void AccelerateMesh(Mesh& mesh) {
if (mesh.accelerator != 0) {
return;
}
vec3 min = mesh.vertices[0];
vec3 max = mesh.vertices[0];
for (int i = 1; i < mesh.numTriangles * 3; ++i) {
min.x = fminf(mesh.vertices[i].x, min.x);
min.y = fminf(mesh.vertices[i].y, min.y);
min.z = fminf(mesh.vertices[i].z, min.z);
max.x = fmaxf(mesh.vertices[i].x, max.x);
max.y = fmaxf(mesh.vertices[i].y, max.y);
max.z = fmaxf(mesh.vertices[i].z, max.z);
}
mesh.accelerator = new BVHNode();
mesh.accelerator->bounds = FromMinMax(min, max);
mesh.accelerator->children = 0;
mesh.accelerator->numTriangles = mesh.numTriangles;
mesh.accelerator->triangles = new int[mesh.numTriangles];
for (int i = 0; i < mesh.numTriangles; ++i) {
mesh.accelerator->triangles[i] = i;
}
SplitBVHNode(mesh.accelerator, mesh, 3);
}
void SplitBVHNode(BVHNode* node, const Mesh& model, int depth) {
if (depth-- <= 0) { // Decrements depth
return;
}
if (node->children == 0) {
// Only split if this node contains triangles
if (node->numTriangles > 0) {
node->children = new BVHNode[8];
vec3 c = node->bounds.position;
vec3 e = node->bounds.size *0.5f;
node->children[0].bounds = AABB(c + vec3(-e.x, +e.y, -e.z), e);
node->children[1].bounds = AABB(c + vec3(+e.x, +e.y, -e.z), e);
node->children[2].bounds = AABB(c + vec3(-e.x, +e.y, +e.z), e);
node->children[3].bounds = AABB(c + vec3(+e.x, +e.y, +e.z), e);
node->children[4].bounds = AABB(c + vec3(-e.x, -e.y, -e.z), e);
node->children[5].bounds = AABB(c + vec3(+e.x, -e.y, -e.z), e);
node->children[6].bounds = AABB(c + vec3(-e.x, -e.y, +e.z), e);
node->children[7].bounds = AABB(c + vec3(+e.x, -e.y, +e.z), e);
}
}
// If this node was just split
if (node->children != 0 && node->numTriangles > 0) {
for (int i = 0; i < 8; ++i) { // For each child
// Count how many triangles each child will contain
node->children[i].numTriangles = 0;
for (int j = 0; j < node->numTriangles; ++j) {
Triangle t = model.triangles[node->triangles[j]];
if (TriangleAABB(t, node->children[i].bounds)) {
node->children[i].numTriangles += 1;
}
}
if (node->children[i].numTriangles == 0) {
continue;
}
node->children[i].triangles = new int[node->children[i].numTriangles];
int index = 0; // Add the triangles in the new child arrau
for (int j = 0; j < node->numTriangles; ++j) {
Triangle t = model.triangles[node->triangles[j]];
if (TriangleAABB(t, node->children[i].bounds)) {
node->children[i].triangles[index++] = node->triangles[j];
}
}
}
node->numTriangles = 0;
delete[] node->triangles;
node->triangles = 0;
// Recurse
for (int i = 0; i < 8; ++i) {
SplitBVHNode(&node->children[i], model, depth);
}
}
}
void FreeBVHNode(BVHNode* node) {
if (node->children != 0) {
for (int i = 0; i < 8; ++i) {
FreeBVHNode(&node->children[i]);
}
delete[] node->children;
node->children = 0;
}
if (node->numTriangles != 0 || node->triangles != 0) {
delete[] node->triangles;
node->triangles = 0;
node->numTriangles = 0;
}
}
bool MeshAABB(const Mesh& mesh, const AABB& aabb) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
if (TriangleAABB(mesh.triangles[i], aabb)) {
return true;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
if (TriangleAABB(mesh.triangles[iterator->triangles[i]], aabb)) {
return true;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
if (AABBAABB(iterator->children[i].bounds, aabb)) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return false;
}
bool Linetest(const Mesh& mesh, const Line& line) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
if (Linetest(mesh.triangles[i], line)) {
return true;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
if (Linetest(mesh.triangles[iterator->triangles[i]], line)) {
return true;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
if (Linetest(iterator->children[i].bounds, line)) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return false;
}
bool MeshSphere(const Mesh& mesh, const Sphere& sphere) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
if (TriangleSphere(mesh.triangles[i], sphere)) {
return true;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
if (TriangleSphere(mesh.triangles[iterator->triangles[i]], sphere)) {
return true;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
if (SphereAABB(sphere, iterator->children[i].bounds)) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return false;
}
bool MeshOBB(const Mesh& mesh, const OBB& obb) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
if (TriangleOBB(mesh.triangles[i], obb)) {
return true;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
if (TriangleOBB(mesh.triangles[iterator->triangles[i]], obb)) {
return true;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
if (AABBOBB(iterator->children[i].bounds, obb)) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return false;
}
bool MeshPlane(const Mesh& mesh, const Plane& plane) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
if (TrianglePlane(mesh.triangles[i], plane)) {
return true;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
if (TrianglePlane(mesh.triangles[iterator->triangles[i]], plane)) {
return true;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
if (AABBPlane(iterator->children[i].bounds, plane)) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return false;
}
bool MeshTriangle(const Mesh& mesh, const Triangle& triangle) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
if (TriangleTriangle(mesh.triangles[i], triangle)) {
return true;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
if (TriangleTriangle(mesh.triangles[iterator->triangles[i]], triangle)) {
return true;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
if (TriangleAABB(triangle, iterator->children[i].bounds)) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return false;
}
float Raycast(const Mesh& mesh, const Ray& ray) {
return MeshRay(mesh, ray);
}
float Raycast(const Model& mesh, const Ray& ray) {
return ModelRay(mesh, ray);
}
float MeshRay(const Mesh& mesh, const Ray& ray) {
if (mesh.accelerator == 0) {
for (int i = 0; i < mesh.numTriangles; ++i) {
RaycastResult raycast;
Raycast(mesh.triangles[i], ray, &raycast);
float result = raycast.t;
if (result >= 0) {
return result;
}
}
}
else {
std::list toProcess;
toProcess.push_front(mesh.accelerator);
// Recursivley walk the BVH tree
while (!toProcess.empty()) {
BVHNode* iterator = *(toProcess.begin());
toProcess.erase(toProcess.begin());
if (iterator->numTriangles >= 0) {
// Iterate trough all triangles of the node
for (int i = 0; i < iterator->numTriangles; ++i) {
// Triangle indices in BVHNode index the mesh
RaycastResult raycast;
Raycast(mesh.triangles[iterator->triangles[i]], ray, &raycast);
float r = raycast.t;
if (r >= 0) {
return r;
}
}
}
if (iterator->children != 0) {
for (int i = 8 - 1; i >= 0; --i) {
// Only push children whos bounds intersect the test geometry
RaycastResult raycast;
Raycast(iterator->children[i].bounds, ray, &raycast);
if (raycast.t >= 0) {
toProcess.push_front(&iterator->children[i]);
}
}
}
}
}
return -1;
}
bool TrianglePlane(const Triangle& t, const Plane& p) {
float side1 = PlaneEquation(t.a, p);
float side2 = PlaneEquation(t.b, p);
float side3 = PlaneEquation(t.c, p);
// On Plane
if (CMP(side1, 0) && CMP(side2, 0) && CMP(side3, 0)) {
return true;
}
// Triangle in front of plane
if (side1 > 0 && side2 > 0 && side3 > 0) {
return false;
}
// Triangle behind plane
if (side1 < 0 && side2 < 0 && side3 < 0) {
return false;
}
return true; // Intersection
}
void Model::SetContent(Mesh* mesh) {
content = mesh;
if (content != 0) {
vec3 min = mesh->vertices[0];
vec3 max = mesh->vertices[0];
for (int i = 1; i < mesh->numTriangles * 3; ++i) {
min.x = fminf(mesh->vertices[i].x, min.x);
min.y = fminf(mesh->vertices[i].y, min.y);
min.z = fminf(mesh->vertices[i].z, min.z);
max.x = fmaxf(mesh->vertices[i].x, max.x);
max.y = fmaxf(mesh->vertices[i].y, max.y);
max.z = fmaxf(mesh->vertices[i].z, max.z);
}
bounds = FromMinMax(min, max);
}
}
mat4 GetWorldMatrix(const Model& model) {
mat4 translation = Translation(model.position);
mat4 rotation = Rotation(model.rotation.x, model.rotation.y, model.rotation.z);
mat4 localMat = /* Scale * */ rotation * translation;
mat4 parentMat;
if (model.parent != 0) {
parentMat = GetWorldMatrix(*model.parent);
}
return localMat * parentMat;
}
OBB GetOBB(const Model& model) {
mat4 world = GetWorldMatrix(model);
AABB aabb = model.GetBounds();
OBB obb;
obb.size = aabb.size;
obb.position = MultiplyPoint(aabb.position, world);
obb.orientation = Cut(world, 3, 3);
return obb;
}
float ModelRay(const Model& model, const Ray& ray) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
Ray local;
local.origin = MultiplyPoint(ray.origin, inv);
local.direction = MultiplyVector(ray.direction, inv);
local.NormalizeDirection();
if (model.GetMesh() != 0) {
return MeshRay(*(model.GetMesh()), local);
}
return -1;
}
bool Linetest(const Model& model, const Line& line) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
Line local;
local.start = MultiplyPoint(line.start, inv);
local.end = MultiplyPoint(line.end, inv);
if (model.GetMesh() != 0) {
return Linetest(*(model.GetMesh()), local);
}
return false;
}
bool ModelSphere(const Model& model, const Sphere& sphere) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
Sphere local;
local.position = MultiplyPoint(sphere.position, inv);
if (model.GetMesh() != 0) {
return MeshSphere(*(model.GetMesh()), local);
}
return false;
}
bool ModelAABB(const Model& model, const AABB& aabb) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
OBB local;
local.size = aabb.size;
local.position = MultiplyPoint(aabb.position, inv);
local.orientation = Cut(inv, 3, 3);
if (model.GetMesh() != 0) {
return MeshOBB(*(model.GetMesh()), local);
}
return false;
}
bool ModelOBB(const Model& model, const OBB& obb) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
OBB local;
local.size = obb.size;
local.position = MultiplyPoint(obb.position, inv);
local.orientation = obb.orientation * Cut(inv, 3, 3);
if (model.GetMesh() != 0) {
return MeshOBB(*(model.GetMesh()), local);
}
return false;
}
bool ModelPlane(const Model& model, const Plane& plane) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
Plane local;
local.normal = MultiplyVector(plane.normal, inv);
local.distance = plane.distance;
if (model.GetMesh() != 0) {
return MeshPlane(*(model.GetMesh()), local);
}
return false;
}
bool ModelTriangle(const Model& model, const Triangle& triangle) {
mat4 world = GetWorldMatrix(model);
mat4 inv = Inverse(world);
Triangle local;
local.a = MultiplyPoint(triangle.a, inv);
local.b = MultiplyPoint(triangle.b, inv);
local.c = MultiplyPoint(triangle.c, inv);
if (model.GetMesh() != 0) {
return MeshTriangle(*(model.GetMesh()), local);
}
return false;
}
Point Intersection(Plane p1, Plane p2, Plane p3) {
/*return ((Cross(p2.normal, p3.normal) * -p1.distance) +
(Cross(p3.normal, p1.normal) * -p2.distance) +
(Cross(p1.normal, p2.normal) * -p3.distance)) /
(Dot(p1.normal, Cross(p2.normal, p3.normal)));*/
#if 1
mat3 D(
p1.normal.x, p2.normal.x, p3.normal.x,
p1.normal.y, p2.normal.y, p3.normal.y,
p1.normal.z, p2.normal.z, p3.normal.z
);
vec3 A(-p1.distance, -p2.distance, -p3.distance);
mat3 Dx = D, Dy = D, Dz = D;
Dx._11 = A.x; Dx._12 = A.y; Dx._13 = A.z;
Dy._21 = A.x; Dy._22 = A.y; Dy._23 = A.z;
Dz._31 = A.x; Dz._32 = A.y; Dz._33 = A.z;
float detD = Determinant(D);
if (CMP(detD, 0)) {
return Point();
}
float detDx = Determinant(Dx);
float detDy = Determinant(Dy);
float detDz = Determinant(Dz);
return Point(detDx / detD, detDy / detD, detDz / detD);
#else
vec3 m1(p1.normal.x, p2.normal.x, p3.normal.x);
vec3 m2(p1.normal.y, p2.normal.y, p3.normal.y);
vec3 m3(p1.normal.z, p2.normal.z, p3.normal.z);
vec3 d(-p1.distance, -p2.distance, -p3.distance);
vec3 u = Cross(m2, m3);
vec3 v = Cross(m1, d);
float denom = Dot(m1, u);
if (CMP(denom, 0.0f)) {
return Point();
}
Point result;
result.x = Dot(d, u) / denom;
result.y = Dot(m3, v) / denom;
result.z = -Dot(m2, v) / denom;
return result;
#endif
}
void GetCorners(const Frustum& f, vec3* outCorners) {
outCorners[0] = Intersection(f._near, f.top, f.left);
outCorners[1] = Intersection(f._near, f.top, f.right);
outCorners[2] = Intersection(f._near, f.bottom, f.left);
outCorners[3] = Intersection(f._near, f.bottom, f.right);
outCorners[4] = Intersection(f._far, f.top, f.left);
outCorners[5] = Intersection(f._far, f.top, f.right);
outCorners[6] = Intersection(f._far, f.bottom, f.left);
outCorners[7] = Intersection(f._far, f.bottom, f.right);
}
bool Intersects(const Frustum& f, const Point& p) {
for (int i = 0; i < 6; ++i) {
vec3 normal = f.planes[i].normal;
float dist = f.planes[i].distance;
float side = Dot(p, normal) + dist;
if (side < 0.0f) {
return false;
}
}
return true;
}
bool Intersects(const Frustum& f, const Sphere& s) {
for (int i = 0; i < 6; ++i) {
vec3 normal = f.planes[i].normal;
float dist = f.planes[i].distance;
float side = Dot(s.position, normal) + dist;
if (side < -s.radius) {
return false;
}
}
return true;
}
float Classify(const AABB& aabb, const Plane& plane) {
// maximum extent in direction of plane normal
float r = fabsf(aabb.size.x * plane.normal.x)
+ fabsf(aabb.size.y * plane.normal.y)
+ fabsf(aabb.size.z * plane.normal.z);
// signed distance between box center and plane
//float d = plane.Test(mCenter);
float d = Dot(plane.normal, aabb.position) + plane.distance;
// return signed distance
if (fabsf(d) < r) {
return 0.0f;
}
else if (d < 0.0f) {
return d + r;
}
return d - r;
}
float Classify(const OBB& obb, const Plane& plane) {
vec3 normal = MultiplyVector(plane.normal, obb.orientation);
// maximum extent in direction of plane normal
float r = fabsf(obb.size.x * normal.x)
+ fabsf(obb.size.y * normal.y)
+ fabsf(obb.size.z * normal.z);
// signed distance between box center and plane
//float d = plane.Test(mCenter);
float d = Dot(plane.normal, obb.position) + plane.distance;
// return signed distance
if (fabsf(d) < r) {
return 0.0f;
}
else if (d < 0.0f) {
return d + r;
}
return d - r;
}
bool Intersects(const Frustum& f, const OBB& obb) {
for (int i = 0; i < 6; ++i) {
float side = Classify(obb, f.planes[i]);
if (side < 0) {
return false;
}
}
return true;
}
bool Intersects(const Frustum& f, const AABB& aabb) {
for (int i = 0; i < 6; ++i) {
float side = Classify(aabb, f.planes[i]);
if (side < 0) {
return false;
}
}
return true;
}
vec3 Unproject(const vec3& viewportPoint, const vec2& viewportOrigin, const vec2& viewportSize, const mat4& view, const mat4& projection) {
// Step 1, Normalize the input vector to the view port
float normalized[4] = {
(viewportPoint.x - viewportOrigin.x) / viewportSize.x,
(viewportPoint.y - viewportOrigin.y) / viewportSize.y,
viewportPoint.z,
1.0f
};
// Step 2, Translate into NDC space
float ndcSpace[4] = {
normalized[0], normalized[1],
normalized[2], normalized[3]
};
// X Range: -1 to 1
ndcSpace[0] = ndcSpace[0] * 2.0f - 1.0f;
// Y Range: -1 to 1, our Y axis is flipped!
ndcSpace[1] = 1.0f - ndcSpace[1] * 2.0f;
// Z Range: 0 to 1
if (ndcSpace[2] < 0.0f) {
ndcSpace[2] = 0.0f;
}
if (ndcSpace[2] > 1.0f) {
ndcSpace[2] = 1.0f;
}
// Step 3, NDC to Eye Space
mat4 invProjection = Inverse(projection);
float eyeSpace[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
// eyeSpace = MultiplyPoint(ndcSpace, invProjection);
Multiply(eyeSpace, ndcSpace, 1, 4, invProjection.asArray, 4, 4);
// Step 4, Eye Space to World Space
mat4 invView = Inverse(view);
float worldSpace[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
// worldSpace = MultiplyPoint(eyeSpace, invView);
Multiply(worldSpace, eyeSpace, 1, 4, invView.asArray, 4, 4);
// Step 5, Undo perspective divide!
if (!CMP(worldSpace[3], 0.0f)) {
worldSpace[0] /= worldSpace[3];
worldSpace[1] /= worldSpace[3];
worldSpace[2] /= worldSpace[3];
}
// Return the resulting world space point
return vec3(worldSpace[0], worldSpace[1], worldSpace[2]);
}
Ray GetPickRay(const vec2& viewportPoint, const vec2& viewportOrigin, const vec2& viewportSize, const mat4& view, const mat4& projection) {
vec3 nearPoint(viewportPoint.x, viewportPoint.y, 0.0f);
vec3 farPoint(viewportPoint.x, viewportPoint.y, 1.0f);
vec3 pNear = Unproject(nearPoint, viewportOrigin, viewportSize, view, projection);
vec3 pFar = Unproject(farPoint, viewportOrigin, viewportSize, view, projection);
vec3 normal = Normalized(pFar - pNear);
vec3 origin = pNear;
return Ray(origin, normal);
}
// Chapter 15
void ResetCollisionManifold(CollisionManifold* result) {
if (result != 0) {
result->colliding = false;
result->normal = vec3(0, 0, 1);
result->depth = FLT_MAX;
if (result->contacts.size() > 0) {
result->contacts.clear();
}
}
}
std::vector GetVertices(const OBB& obb) {
std::vector v;
v.resize(8);
vec3 C = obb.position; // OBB Center
vec3 E = obb.size; // OBB Extents
const float* o = obb.orientation.asArray;
vec3 A[] = { // OBB Axis
vec3(o[0], o[1], o[2]),
vec3(o[3], o[4], o[5]),
vec3(o[6], o[7], o[8]),
};
v[0] = C + A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
v[1] = C - A[0] * E[0] + A[1] * E[1] + A[2] * E[2];
v[2] = C + A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
v[3] = C + A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
v[4] = C - A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
v[5] = C + A[0] * E[0] - A[1] * E[1] - A[2] * E[2];
v[6] = C - A[0] * E[0] + A[1] * E[1] - A[2] * E[2];
v[7] = C - A[0] * E[0] - A[1] * E[1] + A[2] * E[2];
return v;
}
std::vector GetEdges(const OBB& obb) {
std::vector result;
result.reserve(12);
std::vector v = GetVertices(obb);
int index[][2] = { // Indices of edges
{ 6, 1 },{ 6, 3 },{ 6, 4 },{ 2, 7 },{ 2, 5 },{ 2, 0 },
{ 0, 1 },{ 0, 3 },{ 7, 1 },{ 7, 4 },{ 4, 5 },{ 5, 3 }
};
for (int j = 0; j < 12; ++j) {
result.push_back(Line(
v[index[j][0]], v[index[j][1]]
));
}
return result;
}
std::vector GetPlanes(const OBB& obb) {
vec3 c = obb.position; // OBB Center
vec3 e = obb.size; // OBB Extents
const float* o = obb.orientation.asArray;
vec3 a[] = { // OBB Axis
vec3(o[0], o[1], o[2]),
vec3(o[3], o[4], o[5]),
vec3(o[6], o[7], o[8]),
};
std::vector result;
result.resize(6);
result[0] = Plane(a[0] , Dot(a[0], (c + a[0] * e.x)));
result[1] = Plane(a[0] * -1.0f, -Dot(a[0], (c - a[0] * e.x)));
result[2] = Plane(a[1] , Dot(a[1], (c + a[1] * e.y)));
result[3] = Plane(a[1] * -1.0f, -Dot(a[1], (c - a[1] * e.y)));
result[4] = Plane(a[2] , Dot(a[2], (c + a[2] * e.z)));
result[5] = Plane(a[2] * -1.0f, -Dot(a[2], (c - a[2] * e.z)));
return result;
}
bool ClipToPlane(const Plane& plane, const Line& line, Point* outPoint) {
vec3 ab = line.end - line.start;
float nA = Dot(plane.normal, line.start);
float nAB = Dot(plane.normal, ab);
if (CMP(nAB, 0)) {
return false;
}
float t = (plane.distance - nA) / nAB;
if (t >= 0.0f && t <= 1.0f) {
if (outPoint != 0) {
*outPoint = line.start + ab * t;
}
return true;
}
return false;
}
std::vector ClipEdgesToOBB(const std::vector& edges, const OBB& obb) {
std::vector result;
result.reserve(edges.size() * 3);
Point intersection;
std::vector& planes = GetPlanes(obb);
for (int i = 0; i < planes.size(); ++i) {
for (int j = 0; j < edges.size(); ++j) {
if (ClipToPlane(planes[i], edges[j], &intersection)) {
if (PointInOBB(intersection, obb)) {
result.push_back(intersection);
}
}
}
}
return result;
}
float PenetrationDepth(const OBB& o1, const OBB& o2, const vec3& axis, bool* outShouldFlip) {
Interval i1 = GetInterval(o1, Normalized(axis));
Interval i2 = GetInterval(o2, Normalized(axis));
if (!((i2.min <= i1.max) && (i1.min <= i2.max))) {
return 0.0f; // No penerattion
}
float len1 = i1.max - i1.min;
float len2 = i2.max - i2.min;
float min = fminf(i1.min, i2.min);
float max = fmaxf(i1.max, i2.max);
float length = max - min;
if (outShouldFlip != 0) {
*outShouldFlip = (i2.min < i1.min);
}
return (len1 + len2) - length;
}
CollisionManifold FindCollisionFeatures(const OBB& A, const OBB& B) {
CollisionManifold result; // Will return result of intersection!
ResetCollisionManifold(&result);
Sphere s1(A.position, Magnitude(A.size));
Sphere s2(B.position, Magnitude(B.size));
if (!SphereSphere(s1, s2)) {
return result;
}
const float* o1 = A.orientation.asArray;
const float* o2 = B.orientation.asArray;
vec3 test[15] = {
vec3(o1[0], o1[1], o1[2]),
vec3(o1[3], o1[4], o1[5]),
vec3(o1[6], o1[7], o1[8]),
vec3(o2[0], o2[1], o2[2]),
vec3(o2[3], o2[4], o2[5]),
vec3(o2[6], o2[7], o2[8])
};
for (int i = 0; i < 3; ++i) { // Fill out rest of axis
test[6 + i * 3 + 0] = Cross(test[i], test[0]);
test[6 + i * 3 + 1] = Cross(test[i], test[1]);
test[6 + i * 3 + 2] = Cross(test[i], test[2]);
}
vec3* hitNormal = 0;
bool shouldFlip;
for (int i = 0; i < 15; ++i) {
if (test[i].x < 0.000001f) test[i].x = 0.0f;
if (test[i].y < 0.000001f) test[i].y = 0.0f;
if (test[i].z < 0.000001f) test[i].z = 0.0f;
if (MagnitudeSq(test[i])< 0.001f) {
continue;
}
float depth = PenetrationDepth(A, B, test[i], &shouldFlip);
if (depth <= 0.0f) {
return result;
}
else if (depth < result.depth) {
if (shouldFlip) {
test[i] = test[i] * -1.0f;
}
result.depth = depth;
hitNormal = &test[i];
}
}
if (hitNormal == 0) {
return result;
}
vec3 axis = Normalized(*hitNormal);
std::vector c1 = ClipEdgesToOBB(GetEdges(B), A);
std::vector c2 = ClipEdgesToOBB(GetEdges(A), B);
result.contacts.reserve(c1.size() + c2.size());
result.contacts.insert(result.contacts.end(), c1.begin(), c1.end());
result.contacts.insert(result.contacts.end(), c2.begin(), c2.end());
Interval i = GetInterval(A, axis);
float distance = (i.max - i.min)* 0.5f - result.depth * 0.5f;
vec3 pointOnPlane = A.position + axis * distance;
for (int i = result.contacts.size() - 1; i >= 0; --i) {
vec3 contact = result.contacts[i];
result.contacts[i] = contact + (axis * Dot(axis, pointOnPlane - contact));
// This bit is in the "There is more" section of the book
for (int j = result.contacts.size() - 1; j > i; --j) {
if (MagnitudeSq(result.contacts[j] - result.contacts[i]) < 0.0001f) {
result.contacts.erase(result.contacts.begin() + j);
break;
}
}
}
result.colliding = true;
result.normal = axis;
return result;
}
CollisionManifold FindCollisionFeatures(const Sphere& A, const Sphere& B) {
CollisionManifold result; // Will return result of intersection!
ResetCollisionManifold(&result);
float r = A.radius + B.radius;
vec3 d = B.position - A.position;
if (MagnitudeSq(d) - r * r > 0 || MagnitudeSq(d) == 0.0f) {
return result;
}
Normalize(d);
result.colliding = true;
result.normal = d;
result.depth = fabsf(Magnitude(d) - r) * 0.5f;
// dtp - Distance to intersection point
float dtp = A.radius - result.depth;
Point contact = A.position + d * dtp;
result.contacts.push_back(contact);
return result;
}
CollisionManifold FindCollisionFeatures(const OBB& A, const Sphere& B) {
CollisionManifold result; // Will return result of intersection!
ResetCollisionManifold(&result);
Point closestPoint = ClosestPoint(A, B.position);
float distanceSq = MagnitudeSq(closestPoint - B.position);
if (distanceSq > B.radius * B.radius) {
return result;
}
vec3 normal;
if (CMP(distanceSq, 0.0f)) {
if (CMP(MagnitudeSq(closestPoint - A.position), 0.0f)) {
return result;
}
// Closest point is at the center of the sphere
normal = Normalized(closestPoint - A.position);
}
else {
normal = Normalized(B.position - closestPoint);
}
Point outsidePoint = B.position - normal * B.radius;
float distance = Magnitude(closestPoint - outsidePoint);
result.colliding = true;
result.contacts.push_back(closestPoint + (outsidePoint - closestPoint) * 0.5f);
result.normal = normal;
result.depth = distance * 0.5f;
return result;
}
game-physics-cookbook----Vector.cpp
#include "Compare.h"
#include "vectors.h"
#include
#include
float CorrectDegrees(float degrees) {
while (degrees > 360.0f) {
degrees -= 360.0f;
}
while (degrees < -360.0f) {
degrees += 360.0f;
}
return degrees;
}
#ifndef RAD2DEG
float RAD2DEG(float radians) {
float degrees = radians * 57.295754f;
degrees = CorrectDegrees(degrees);
return degrees;
}
#endif
#ifndef DEG2RAD
float DEG2RAD(float degrees) {
degrees = CorrectDegrees(degrees);
float radians = degrees * 0.0174533f;
return radians;
}
#endif
bool operator==(const vec2& l, const vec2& r) {
return CMP(l.x, r.x) && CMP(l.y, r.y);
}
bool operator==(const vec3& l, const vec3& r) {
return CMP(l.x, r.x) && CMP(l.y, r.y) && CMP(l.z, r.z);
}
bool operator!=(const vec2& l, const vec2& r) {
return !(l == r);
}
bool operator!=(const vec3& l, const vec3& r) {
return !(l == r);
}
vec2 operator+(const vec2& l, const vec2& r) {
return { l.x + r.x, l.y + r.y };
}
vec3 operator+(const vec3& l, const vec3& r) {
return { l.x + r.x, l.y + r.y, l.z + r.z };
}
vec2 operator-(const vec2& l, const vec2& r) {
return { l.x - r.x, l.y - r.y };
}
vec3 operator-(const vec3& l, const vec3& r) {
return { l.x - r.x, l.y - r.y, l.z - r.z };
}
vec2 operator*(const vec2& l, const vec2& r) {
return { l.x * r.x, l.y * r.y };
}
vec3 operator*(const vec3& l, const vec3& r) {
return { l.x * r.x, l.y * r.y, l.z * r.z };
}
vec2 operator*(const vec2& l, float r) {
return { l.x * r, l.y * r };
}
vec3 operator*(const vec3& l, float r) {
return { l.x * r, l.y * r, l.z * r };
}
vec2 operator/(const vec2& l, const vec2& r) {
return{ l.x / r.x, l.y / r.y };
}
vec3 operator/(const vec3& l, const vec3& r) {
return{ l.x / r.x, l.y / r.y, l.z / r.z };
}
vec2 operator/(const vec2& l, float r) {
return{ l.x / r, l.y / r };
}
vec3 operator/(const vec3& l, float r) {
return{ l.x / r, l.y / r, l.z / r };
}
std::ostream& operator<<(std::ostream& os, const vec2& m) {
os << "(" << m.x << ", " << m.y << ")";
return os;
}
std::ostream& operator<<(std::ostream& os, const vec3& m) {
os << "(" << m.x << ", " << m.y << ", " << m.z << ")";
return os;
}
float Dot(const vec2& l, const vec2& r) {
return l.x * r.x + l.y * r.y;
}
float Dot(const vec3& l, const vec3& r) {
return l.x * r.x + l.y * r.y + l.z * r.z;
}
vec2& operator+=(vec2& l, const vec2& r) {
l.x += r.x;
l.y += r.y;
return l;
}
vec2& operator-=(vec2& l, const vec2& r) {
l.x -= r.y;
l.y -= r.y;
return l;
}
vec2& operator*=(vec2& l, const vec2& r) {
l.x *= r.x;
l.y *= r.y;
return l;
}
vec2& operator*=(vec2& l, const float r) {
l.x *= r;
l.y *= r;
return l;
}
vec2& operator/=(vec2& l, const vec2& r) {
l.x /= r.x;
l.y /= r.y;
return l;
}
vec2& operator/=(vec2& l, const float r) {
l.x /= r;
l.y /= r;
return l;
}
vec3& operator+=(vec3& l, const vec3& r) {
l.x += r.x;
l.y += r.y;
l.z += r.z;
return l;
}
vec3& operator-=(vec3& l, const vec3& r) {
l.x -= r.x;
l.y -= r.y;
l.z -= r.z;
return l;
}
vec3& operator*=(vec3& l, const vec3& r) {
l.x *= r.x;
l.y *= r.y;
l.z *= r.z;
return l;
}
vec3& operator*=(vec3& l, const float r) {
l.x *= r;
l.y *= r;
l.z *= r;
return l;
}
vec3& operator/=(vec3& l, const vec3& r) {
l.x /= r.x;
l.y /= r.y;
l.z /= r.z;
return l;
}
vec3& operator/=(vec3& l, const float r) {
l.x /= r;
l.y /= r;
l.z /= r;
return l;
}
float Magnitude(const vec2& v) {
return sqrtf(Dot(v, v));
}
float Magnitude(const vec3& v) {
return sqrtf(Dot(v, v));
}
float MagnitudeSq(const vec2& v) {
return Dot(v, v);
}
float MagnitudeSq(const vec3& v) {
return Dot(v, v);
}
float Distance(const vec2& p1, const vec2& p2) {
return Magnitude(p1 - p2);
}
float Distance(const vec3& p1, const vec3& p2) {
return Magnitude(p1 - p2);
}
float DistanceSq(const vec2& p1, const vec2& p2) {
return MagnitudeSq(p1 - p2);
}
float DistanceSq(const vec3& p1, const vec3& p2) {
return MagnitudeSq(p1 - p2);
}
vec2 RotateVector(const vec2& vector, float degrees) {
degrees = DEG2RAD(degrees);
float s = sinf(degrees);
float c = cosf(degrees);
return vec2(
vector.x * c - vector.y * s,
vector.x * s + vector.y * c
);
}
void Normalize(vec2& v) {
v = v * (1.0f / Magnitude(v));
}
void Normalize(vec3& v) {
v = v * (1.0f / Magnitude(v));
}
vec2 Normalized(const vec2& v) {
return v * (1.0f / Magnitude(v));
}
vec3 Normalized(const vec3& v) {
return v * (1.0f / Magnitude(v));
}
vec3 Cross(const vec3& l, const vec3& r) {
vec3 result;
result.x = l.y * r.z - l.z * r.y;
result.y = l.z * r.x - l.x * r.z;
result.z = l.x * r.y - l.y * r.x;
return result;
}
float Angle(const vec2& l, const vec2& r) {
return acosf(Dot(l, r) / sqrtf(MagnitudeSq(l) * MagnitudeSq(r)));
}
float Angle(const vec3& l, const vec3& r) {
return acosf(Dot(l, r) / sqrtf(MagnitudeSq(l) * MagnitudeSq(r)));
}
vec2 Project(const vec2& length, const vec2& direction) {
float dot = Dot(length, direction);
float magSq = MagnitudeSq(direction);
return direction * (dot / magSq);
}
vec3 Project(const vec3& length, const vec3& direction) {
float dot = Dot(length, direction);
float magSq = MagnitudeSq(direction);
return direction * (dot / magSq);
}
vec2 Perpendicular(const vec2& length, const vec2& direction) {
return length - Project(length, direction);
}
vec3 Perpendicular(const vec3& length, const vec3& direction) {
return length - Project(length, direction);
}
vec2 Reflection(const vec2& sourceVector, const vec2& normal) {
return sourceVector - normal * (Dot(sourceVector, normal) * 2.0f );
}
vec3 Reflection(const vec3& sourceVector, const vec3& normal) {
return sourceVector - normal * (Dot(sourceVector, normal) * 2.0f);
}
Blender----vectorbase.h
/******************************************************************************
*
* MantaFlow fluid solver framework
* Copyright 2011-2016 Tobias Pfaff, Nils Thuerey
*
* This program is free software, distributed under the terms of the
* Apache License, Version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Basic vector class
*
******************************************************************************/
#ifndef _VECTORBASE_H
#define _VECTORBASE_H
// get rid of windos min/max defines
#if (defined(WIN32) || defined(_WIN32)) && !defined(NOMINMAX)
# define NOMINMAX
#endif
#include
#include
#include
#include
#include "general.h"
// if min/max are still around...
#if defined(WIN32) || defined(_WIN32)
# undef min
# undef max
#endif
// redefine usage of some windows functions
#if defined(WIN32) || defined(_WIN32)
# ifndef snprintf
# define snprintf _snprintf
# endif
#endif
// use which fp-precision? 1=float, 2=double
#ifndef FLOATINGPOINT_PRECISION
# define FLOATINGPOINT_PRECISION 1
#endif
// VECTOR_EPSILON is the minimal vector length
// In order to be able to discriminate floating point values near zero, and
// to be sure not to fail a comparison because of roundoff errors, use this
// value as a threshold.
#if FLOATINGPOINT_PRECISION == 1
typedef float Real;
# define VECTOR_EPSILON (1e-6f)
#else
typedef double Real;
# define VECTOR_EPSILON (1e-10)
#endif
#ifndef M_PI
# define M_PI 3.1415926536
#endif
#ifndef M_E
# define M_E 2.7182818284
#endif
namespace Manta {
//! Basic inlined vector class
template class Vector3D {
public:
//! Constructor
inline Vector3D() : x(0), y(0), z(0)
{
}
//! Copy-Constructor
inline Vector3D(const Vector3D &v) : x(v.x), y(v.y), z(v.z)
{
}
//! Copy-Constructor
inline Vector3D(const int *v) : x((S)v[0]), y((S)v[1]), z((S)v[2])
{
}
//! Copy-Constructor
inline Vector3D(const float *v) : x((S)v[0]), y((S)v[1]), z((S)v[2])
{
}
//! Copy-Constructor
inline Vector3D(const double *v) : x((S)v[0]), y((S)v[1]), z((S)v[2])
{
}
//! Construct a vector from one S
inline Vector3D(S v) : x(v), y(v), z(v)
{
}
//! Construct a vector from three Ss
inline Vector3D(S vx, S vy, S vz) : x(vx), y(vy), z(vz)
{
}
// Operators
//! Assignment operator
inline const Vector3D &operator=(const Vector3D &v)
{
x = v.x;
y = v.y;
z = v.z;
return *this;
}
//! Assignment operator
inline const Vector3D &operator=(S s)
{
x = y = z = s;
return *this;
}
//! Assign and add operator
inline const Vector3D &operator+=(const Vector3D &v)
{
x += v.x;
y += v.y;
z += v.z;
return *this;
}
//! Assign and add operator
inline const Vector3D &operator+=(S s)
{
x += s;
y += s;
z += s;
return *this;
}
//! Assign and sub operator
inline const Vector3D &operator-=(const Vector3D &v)
{
x -= v.x;
y -= v.y;
z -= v.z;
return *this;
}
//! Assign and sub operator
inline const Vector3D &operator-=(S s)
{
x -= s;
y -= s;
z -= s;
return *this;
}
//! Assign and mult operator
inline const Vector3D &operator*=(const Vector3D &v)
{
x *= v.x;
y *= v.y;
z *= v.z;
return *this;
}
//! Assign and mult operator
inline const Vector3D &operator*=(S s)
{
x *= s;
y *= s;
z *= s;
return *this;
}
//! Assign and div operator
inline const Vector3D &operator/=(const Vector3D &v)
{
x /= v.x;
y /= v.y;
z /= v.z;
return *this;
}
//! Assign and div operator
inline const Vector3D &operator/=(S s)
{
x /= s;
y /= s;
z /= s;
return *this;
}
//! Negation operator
inline Vector3D operator-() const
{
return Vector3D(-x, -y, -z);
}
//! Get smallest component
inline S min() const
{
return (x < y) ? ((x < z) ? x : z) : ((y < z) ? y : z);
}
//! Get biggest component
inline S max() const
{
return (x > y) ? ((x > z) ? x : z) : ((y > z) ? y : z);
}
//! Test if all components are zero
inline bool empty()
{
return x == 0 && y == 0 && z == 0;
}
//! access operator
inline S &operator[](unsigned int i)
{
return value[i];
}
//! constant access operator
inline const S &operator[](unsigned int i) const
{
return value[i];
}
//! debug output vector to a string
std::string toString() const;
//! test if nans are present
bool isValid() const;
//! actual values
union {
S value[3];
struct {
S x;
S y;
S z;
};
struct {
S X;
S Y;
S Z;
};
};
//! zero element
static const Vector3D Zero, Invalid;
//! For compatibility with 4d vectors (discards 4th comp)
inline Vector3D(S vx, S vy, S vz, S vDummy) : x(vx), y(vy), z(vz)
{
}
protected:
};
//! helper to check whether value is non-zero
template inline bool notZero(S v)
{
return (std::abs(v) > VECTOR_EPSILON);
}
template inline bool notZero(Vector3D v)
{
return (std::abs(norm(v)) > VECTOR_EPSILON);
}
//************************************************************************
// Additional operators
//************************************************************************
//! Addition operator
template inline Vector3D operator+(const Vector3D &v1, const Vector3D &v2)
{
return Vector3D(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
}
//! Addition operator
template inline Vector3D operator+(const Vector3D &v, S2 s)
{
return Vector3D(v.x + s, v.y + s, v.z + s);
}
//! Addition operator
template inline Vector3D operator+(S2 s, const Vector3D &v)
{
return Vector3D(v.x + s, v.y + s, v.z + s);
}
//! Subtraction operator
template inline Vector3D operator-(const Vector3D &v1, const Vector3D &v2)
{
return Vector3D(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}
//! Subtraction operator
template inline Vector3D operator-(const Vector3D &v, S2 s)
{
return Vector3D(v.x - s, v.y - s, v.z - s);
}
//! Subtraction operator
template inline Vector3D operator-(S2 s, const Vector3D &v)
{
return Vector3D(s - v.x, s - v.y, s - v.z);
}
//! Multiplication operator
template inline Vector3D operator*(const Vector3D &v1, const Vector3D &v2)
{
return Vector3D(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
}
//! Multiplication operator
template inline Vector3D operator*(const Vector3D &v, S2 s)
{
return Vector3D(v.x * s, v.y * s, v.z * s);
}
//! Multiplication operator
template inline Vector3D operator*(S2 s, const Vector3D &v)
{
return Vector3D(s * v.x, s * v.y, s * v.z);
}
//! Division operator
template inline Vector3D operator/(const Vector3D &v1, const Vector3D &v2)
{
return Vector3D(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z);
}
//! Division operator
template inline Vector3D operator/(const Vector3D &v, S2 s)
{
return Vector3D(v.x / s, v.y / s, v.z / s);
}
//! Division operator
template inline Vector3D operator/(S2 s, const Vector3D &v)
{
return Vector3D(s / v.x, s / v.y, s / v.z);
}
//! Comparison operator
template inline bool operator==(const Vector3D &s1, const Vector3D &s2)
{
return s1.x == s2.x && s1.y == s2.y && s1.z == s2.z;
}
//! Comparison operator
template inline bool operator!=(const Vector3D &s1, const Vector3D &s2)
{
return s1.x != s2.x || s1.y != s2.y || s1.z != s2.z;
}
//************************************************************************
// External functions
//************************************************************************
//! Min operator
template inline Vector3D vmin(const Vector3D &s1, const Vector3D &s2)
{
return Vector3D(std::min(s1.x, s2.x), std::min(s1.y, s2.y), std::min(s1.z, s2.z));
}
//! Min operator
template inline Vector3D vmin(const Vector3D &s1, S2 s2)
{
return Vector3D(std::min(s1.x, s2), std::min(s1.y, s2), std::min(s1.z, s2));
}
//! Min operator
template inline Vector3D vmin(S1 s1, const Vector3D &s2)
{
return Vector3D(std::min(s1, s2.x), std::min(s1, s2.y), std::min(s1, s2.z));
}
//! Max operator
template inline Vector3D vmax(const Vector3D &s1, const Vector3D &s2)
{
return Vector3D(std::max(s1.x, s2.x), std::max(s1.y, s2.y), std::max(s1.z, s2.z));
}
//! Max operator
template inline Vector3D vmax(const Vector3D &s1, S2 s2)
{
return Vector3D(std::max(s1.x, s2), std::max(s1.y, s2), std::max(s1.z, s2));
}
//! Max operator
template inline Vector3D vmax(S1 s1, const Vector3D &s2)
{
return Vector3D(std::max(s1, s2.x), std::max(s1, s2.y), std::max(s1, s2.z));
}
//! Dot product
template inline S dot(const Vector3D &t, const Vector3D &v)
{
return t.x * v.x + t.y * v.y + t.z * v.z;
}
//! Cross product
template inline Vector3D cross(const Vector3D &t, const Vector3D &v)
{
Vector3D cp(
((t.y * v.z) - (t.z * v.y)), ((t.z * v.x) - (t.x * v.z)), ((t.x * v.y) - (t.y * v.x)));
return cp;
}
//! Project a vector into a plane, defined by its normal
/*! Projects a vector into a plane normal to the given vector, which must
have unit length. Self is modified.
\param v The vector to project
\param n The plane normal
\return The projected vector */
template
inline const Vector3D &projectNormalTo(const Vector3D &v, const Vector3D &n)
{
S sprod = dot(v, n);
return v - n * dot(v, n);
}
//! Compute the magnitude (length) of the vector
//! (clamps to 0 and 1 with VECTOR_EPSILON)
template inline S norm(const Vector3D &v)
{
S l = v.x * v.x + v.y * v.y + v.z * v.z;
if (l <= VECTOR_EPSILON * VECTOR_EPSILON)
return (0.);
return (fabs(l - 1.) < VECTOR_EPSILON * VECTOR_EPSILON) ? 1. : sqrt(l);
}
//! Compute squared magnitude
template inline S normSquare(const Vector3D &v)
{
return v.x * v.x + v.y * v.y + v.z * v.z;
}
//! compatibility, allow use of int, Real and Vec inputs with norm/normSquare
inline Real norm(const Real v)
{
return fabs(v);
}
inline Real normSquare(const Real v)
{
return square(v);
}
inline Real norm(const int v)
{
return abs(v);
}
inline Real normSquare(const int v)
{
return square(v);
}
//! Compute sum of all components, allow use of int, Real too
template inline S sum(const S v)
{
return v;
}
template inline S sum(const Vector3D &v)
{
return v.x + v.y + v.z;
}
//! Get absolute representation of vector, allow use of int, Real too
inline Real abs(const Real v)
{
return std::fabs(v);
}
inline int abs(const int v)
{
return std::abs(v);
}
template inline Vector3D abs(const Vector3D &v)
{
Vector3D cp(v.x, v.y, v.z);
for (int i = 0; i < 3; ++i) {
if (cp[i] < 0)
cp[i] *= (-1.0);
}
return cp;
}
//! Returns a normalized vector
template inline Vector3D getNormalized(const Vector3D &v)
{
S l = v.x * v.x + v.y * v.y + v.z * v.z;
if (fabs(l - 1.) < VECTOR_EPSILON * VECTOR_EPSILON)
return v; /* normalized "enough"... */
else if (l > VECTOR_EPSILON * VECTOR_EPSILON) {
S fac = 1. / sqrt(l);
return Vector3D(v.x * fac, v.y * fac, v.z * fac);
}
else
return Vector3D((S)0);
}
//! Compute the norm of the vector and normalize it.
/*! \return The value of the norm */
template inline S normalize(Vector3D &v)
{
S norm;
S l = v.x * v.x + v.y * v.y + v.z * v.z;
if (fabs(l - 1.) < VECTOR_EPSILON * VECTOR_EPSILON) {
norm = 1.;
}
else if (l > VECTOR_EPSILON * VECTOR_EPSILON) {
norm = sqrt(l);
v *= 1. / norm;
}
else {
v = Vector3D::Zero;
norm = 0.;
}
return (S)norm;
}
//! Obtain an orthogonal vector
/*! Compute a vector that is orthonormal to the given vector.
* Nothing else can be assumed for the direction of the new vector.
* \return The orthonormal vector */
template Vector3D getOrthogonalVector(const Vector3D &v)
{
// Determine the component with max. absolute value
int maxIndex = (fabs(v.x) > fabs(v.y)) ? 0 : 1;
maxIndex = (fabs(v[maxIndex]) > fabs(v.z)) ? maxIndex : 2;
// Choose another axis than the one with max. component and project
// orthogonal to self
Vector3D o(0.0);
o[(maxIndex + 1) % 3] = 1;
Vector3D c = cross(v, o);
normalize(c);
return c;
}
//! Convert vector to polar coordinates
/*! Stable vector to angle conversion
*\param v vector to convert
\param phi unique angle [0,2PI]
\param theta unique angle [0,PI]
*/
template inline void vecToAngle(const Vector3D &v, S &phi, S &theta)
{
if (fabs(v.y) < VECTOR_EPSILON)
theta = M_PI / 2;
else if (fabs(v.x) < VECTOR_EPSILON && fabs(v.z) < VECTOR_EPSILON)
theta = (v.y >= 0) ? 0 : M_PI;
else
theta = atan(sqrt(v.x * v.x + v.z * v.z) / v.y);
if (theta < 0)
theta += M_PI;
if (fabs(v.x) < VECTOR_EPSILON)
phi = M_PI / 2;
else
phi = atan(v.z / v.x);
if (phi < 0)
phi += M_PI;
if (fabs(v.z) < VECTOR_EPSILON)
phi = (v.x >= 0) ? 0 : M_PI;
else if (v.z < 0)
phi += M_PI;
}
//! Compute vector reflected at a surface
/*! Compute a vector, that is self (as an incoming vector)
* reflected at a surface with a distinct normal vector.
* Note that the normal is reversed, if the scalar product with it is positive.
\param t The incoming vector
\param n The surface normal
\return The new reflected vector
*/
template inline Vector3D reflectVector(const Vector3D &t, const Vector3D &n)
{
Vector3D nn = (dot(t, n) > 0.0) ? (n * -1.0) : n;
return (t - nn * (2.0 * dot(nn, t)));
}
//! Compute vector refracted at a surface
/*! \param t The incoming vector
* \param n The surface normal
* \param nt The "inside" refraction index
* \param nair The "outside" refraction index
* \param refRefl Set to 1 on total reflection
* \return The refracted vector
*/
template
inline Vector3D refractVector(
const Vector3D &t, const Vector3D &normal, S nt, S nair, int &refRefl)
{
// from Glassner's book, section 5.2 (Heckberts method)
S eta = nair / nt;
S n = -dot(t, normal);
S tt = 1.0 + eta * eta * (n * n - 1.0);
if (tt < 0.0) {
// we have total reflection!
refRefl = 1;
}
else {
// normal reflection
tt = eta * n - sqrt(tt);
return (t * eta + normal * tt);
}
return t;
}
//! Outputs the object in human readable form as string
template std::string Vector3D::toString() const
{
char buf[256];
snprintf(buf,
256,
"[%+4.6f,%+4.6f,%+4.6f]",
(double)(*this)[0],
(double)(*this)[1],
(double)(*this)[2]);
// for debugging, optionally increase precision:
// snprintf ( buf,256,"[%+4.16f,%+4.16f,%+4.16f]", ( double ) ( *this ) [0], ( double ) ( *this )
// [1], ( double ) ( *this ) [2] );
return std::string(buf);
}
template<> std::string Vector3D::toString() const;
//! Outputs the object in human readable form to stream
/*! Output format [x,y,z] */
template std::ostream &operator<<(std::ostream &os, const Vector3D &i)
{
os << i.toString();
return os;
}
//! Reads the contents of the object from a stream
/*! Input format [x,y,z] */
template std::istream &operator>>(std::istream &is, Vector3D &i)
{
char c;
char dummy[3];
is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> c;
return is;
}
/**************************************************************************/
// Define default vector alias
/**************************************************************************/
//! 3D vector class of type Real (typically float)
typedef Vector3D Vec3;
//! 3D vector class of type int
typedef Vector3D Vec3i;
//! convert to Real Vector
template inline Vec3 toVec3(T v)
{
return Vec3(v[0], v[1], v[2]);
}
//! convert to int Vector
template inline Vec3i toVec3i(T v)
{
return Vec3i((int)v[0], (int)v[1], (int)v[2]);
}
//! convert to int Vector
template inline Vec3i toVec3i(T v0, T v1, T v2)
{
return Vec3i((int)v0, (int)v1, (int)v2);
}
//! round, and convert to int Vector
template inline Vec3i toVec3iRound(T v)
{
return Vec3i((int)round(v[0]), (int)round(v[1]), (int)round(v[2]));
}
template inline Vec3i toVec3iFloor(T v)
{
return Vec3i((int)floor(v[0]), (int)floor(v[1]), (int)floor(v[2]));
}
//! convert to int Vector if values are close enough to an int
template inline Vec3i toVec3iChecked(T v)
{
Vec3i ret;
for (size_t i = 0; i < 3; i++) {
Real a = v[i];
if (fabs(a - floor(a + 0.5)) > 1e-5)
errMsg("argument is not an int, cannot convert");
ret[i] = (int)(a + 0.5);
}
return ret;
}
//! convert to double Vector
template inline Vector3D toVec3d(T v)
{
return Vector3D(v[0], v[1], v[2]);
}
//! convert to float Vector
template inline Vector3D toVec3f(T v)
{
return Vector3D(v[0], v[1], v[2]);
}
/**************************************************************************/
// Specializations for common math functions
/**************************************************************************/
template<> inline Vec3 clamp(const Vec3 &a, const Vec3 &b, const Vec3 &c)
{
return Vec3(clamp(a.x, b.x, c.x), clamp(a.y, b.y, c.y), clamp(a.z, b.z, c.z));
}
template<> inline Vec3 safeDivide(const Vec3 &a, const Vec3 &b)
{
return Vec3(safeDivide(a.x, b.x), safeDivide(a.y, b.y), safeDivide(a.z, b.z));
}
template<> inline Vec3 nmod(const Vec3 &a, const Vec3 &b)
{
return Vec3(nmod(a.x, b.x), nmod(a.y, b.y), nmod(a.z, b.z));
}
}; // namespace Manta
#endif
Unreal Engine5----Vector.h
// Copyright Epic Games, Inc. All Rights Reserved.
#pragma once
#include "CoreTypes.h"
#include "Misc/AssertionMacros.h"
#include "Math/NumericLimits.h"
#include "Misc/Crc.h"
#include "Math/UnrealMathUtility.h"
#include "Containers/UnrealString.h"
#include "Misc/Parse.h"
#include "Misc/LargeWorldCoordinatesSerializer.h"
#include "Misc/NetworkVersion.h"
#include "Math/Color.h"
#include "Math/IntPoint.h"
#include "Logging/LogMacros.h"
#include "Math/Vector2D.h"
#include "Misc/ByteSwap.h"
#include "Internationalization/Text.h"
#include "Internationalization/Internationalization.h"
#include "Math/IntVector.h"
#include "Math/Axis.h"
#include "Serialization/MemoryLayout.h"
#include "UObject/ObjectVersion.h"
#include
#if PLATFORM_ENABLE_VECTORINTRINSICS
#include "Math/VectorRegister.h"
#endif
#ifdef _MSC_VER
#pragma warning (push)
// Ensure template functions don't generate shadowing warnings against global variables at the point of instantiation.
#pragma warning (disable : 4459)
#endif
// Move out of global namespace to avoid collisions with Chaos::TVector within the physics code.
namespace UE
{
namespace Math
{
template struct TPlane;
/**
* A vector in 3-D space composed of components (X, Y, Z) with floating point precision.
*/
template
struct TVector
{
static_assert(std::is_floating_point_v, "T must be floating point");
public:
using FReal = T;
union
{
struct
{
/** Vector's X component. */
T X;
/** Vector's Y component. */
T Y;
/** Vector's Z component. */
T Z;
};
UE_DEPRECATED(all, "For internal use only")
T XYZ[3];
};
/** A zero vector (0,0,0) */
CORE_API static const TVector ZeroVector;
/** One vector (1,1,1) */
CORE_API static const TVector OneVector;
/** Unreal up vector (0,0,1) */
CORE_API static const TVector UpVector;
/** Unreal down vector (0,0,-1) */
CORE_API static const TVector DownVector;
/** Unreal forward vector (1,0,0) */
CORE_API static const TVector ForwardVector;
/** Unreal backward vector (-1,0,0) */
CORE_API static const TVector BackwardVector;
/** Unreal right vector (0,1,0) */
CORE_API static const TVector RightVector;
/** Unreal left vector (0,-1,0) */
CORE_API static const TVector LeftVector;
/** Unit X axis vector (1,0,0) */
CORE_API static const TVector XAxisVector;
/** Unit Y axis vector (0,1,0) */
CORE_API static const TVector YAxisVector;
/** Unit Z axis vector (0,0,1) */
CORE_API static const TVector ZAxisVector;
/** @return Zero Vector (0,0,0) */
static inline TVector Zero() { return ZeroVector; }
/** @return One Vector (1,1,1) */
static inline TVector One() { return OneVector; }
/** @return Unit X Vector (1,0,0) */
static inline TVector UnitX() { return XAxisVector; }
/** @return Unit Y Vector (0,1,0) */
static inline TVector UnitY() { return YAxisVector; }
/** @return Unit Z Vector (0,0,1) */
static inline TVector UnitZ() { return ZAxisVector; }
public:
#if ENABLE_NAN_DIAGNOSTIC
FORCEINLINE void DiagnosticCheckNaN() const
{
if (ContainsNaN())
{
logOrEnsureNanError(TEXT("FVector contains NaN: %s"), *ToString());
*const_cast*>(static_cast*>(this)) = ZeroVector;
}
}
FORCEINLINE void DiagnosticCheckNaN(const TCHAR* Message) const
{
if (ContainsNaN())
{
logOrEnsureNanError(TEXT("%s: FVector contains NaN: %s"), Message, *ToString());
*const_cast*>(static_cast*>(this)) = ZeroVector;
}
}
#else
FORCEINLINE void DiagnosticCheckNaN() const {}
FORCEINLINE void DiagnosticCheckNaN(const TCHAR* Message) const {}
#endif
/** Default constructor (no initialization). */
FORCEINLINE TVector();
/**
* Constructor initializing all components to a single T value.
*
* @param InF Value to set all components to.
*/
explicit FORCEINLINE TVector(T InF);
/**
* Constructor using initial values for each component.
*
* @param InX X Coordinate.
* @param InY Y Coordinate.
* @param InZ Z Coordinate.
*/
FORCEINLINE TVector(T InX, T InY, T InZ);
/**
* Constructs a vector from an TVector2 and Z value.
*
* @param V Vector to copy from.
* @param InZ Z Coordinate.
*/
explicit FORCEINLINE TVector(const TVector2 V, T InZ);
/**
* Constructor using the XYZ components from a 4D vector.
*
* @param V 4D Vector to copy from.
*/
FORCEINLINE TVector(const UE::Math::TVector4& V);
/**
* Constructs a vector from an FLinearColor.
*
* @param InColor Color to copy from.
*/
explicit TVector(const FLinearColor& InColor);
/**
* Constructs a vector from an FIntVector.
*
* @param InVector FIntVector to copy from.
*/
explicit TVector(FIntVector InVector);
/**
* Constructs a vector from an FIntPoint.
*
* @param A Int Point used to set X and Y coordinates, Z is set to zero.
*/
explicit TVector(FIntPoint A);
/**
* Constructor which initializes all components to zero.
*
* @param EForceInit Force init enum
*/
explicit FORCEINLINE TVector(EForceInit);
#ifdef IMPLEMENT_ASSIGNMENT_OPERATOR_MANUALLY
/**
* Copy another TVector into this one
*
* @param Other The other vector.
* @return Reference to vector after copy.
*/
FORCEINLINE TVector& operator=(const TVector& Other);
#endif
/**
* Calculate cross product between this and another vector.
*
* @param V The other vector.
* @return The cross product.
*/
FORCEINLINE TVector operator^(const TVector& V) const;
/**
* Calculate cross product between this and another vector.
*
* @param V The other vector.
* @return The cross product.
*/
FORCEINLINE TVector Cross(const TVector& V2) const;
/**
* Calculate the cross product of two vectors.
*
* @param A The first vector.
* @param B The second vector.
* @return The cross product.
*/
FORCEINLINE static TVector CrossProduct(const TVector& A, const TVector& B);
/**
* Calculate the dot product between this and another vector.
*
* @param V The other vector.
* @return The dot product.
*/
FORCEINLINE T operator|(const TVector& V) const;
/**
* Calculate the dot product between this and another vector.
*
* @param V The other vector.
* @return The dot product.
*/
FORCEINLINE T Dot(const TVector& V) const;
/**
* Calculate the dot product of two vectors.
*
* @param A The first vector.
* @param B The second vector.
* @return The dot product.
*/
FORCEINLINE static T DotProduct(const TVector& A, const TVector& B);
/**
* Gets the result of component-wise addition of this and another vector.
*
* @param V The vector to add to this.
* @return The result of vector addition.
*/
FORCEINLINE TVector operator+(const TVector& V) const;
/**
* Gets the result of component-wise subtraction of this by another vector.
*
* @param V The vector to subtract from this.
* @return The result of vector subtraction.
*/
FORCEINLINE TVector operator-(const TVector& V) const;
/**
* Gets the result of subtracting from each component of the vector.
*
* @param Bias How much to subtract from each component.
* @return The result of subtraction.
*/
template::value)>
FORCEINLINE TVector operator-(FArg Bias) const
{
return TVector(X - (T)Bias, Y - (T)Bias, Z - (T)Bias);
}
/**
* Gets the result of adding to each component of the vector.
*
* @param Bias How much to add to each component.
* @return The result of addition.
*/
template::value)>
FORCEINLINE TVector operator+(FArg Bias) const
{
return TVector(X + (T)Bias, Y + (T)Bias, Z + (T)Bias);
}
/**
* Gets the result of scaling the vector (multiplying each component by a value).
*
* @param Scale What to multiply each component by.
* @return The result of multiplication.
*/
template::value)>
FORCEINLINE TVector operator*(FArg Scale) const
{
return TVector(X * (T)Scale, Y * (T)Scale, Z * (T)Scale);
}
/**
* Gets the result of dividing each component of the vector by a value.
*
* @param Scale What to divide each component by.
* @return The result of division.
*/
template::value)>
TVector operator/(FArg Scale) const
{
const T RScale = T(1) / Scale;
return TVector(X * RScale, Y * RScale, Z * RScale);
}
/**
* Gets the result of component-wise multiplication of this vector by another.
*
* @param V The vector to multiply with.
* @return The result of multiplication.
*/
FORCEINLINE TVector operator*(const TVector& V) const;
/**
* Gets the result of component-wise division of this vector by another.
*
* @param V The vector to divide by.
* @return The result of division.
*/
FORCEINLINE TVector operator/(const TVector& V) const;
// Binary comparison operators.
/**
* Check against another vector for equality.
*
* @param V The vector to check against.
* @return true if the vectors are equal, false otherwise.
*/
bool operator==(const TVector& V) const;
/**
* Check against another vector for inequality.
*
* @param V The vector to check against.
* @return true if the vectors are not equal, false otherwise.
*/
bool operator!=(const TVector& V) const;
/**
* Check against another vector for equality, within specified error limits.
*
* @param V The vector to check against.
* @param Tolerance Error tolerance.
* @return true if the vectors are equal within tolerance limits, false otherwise.
*/
bool Equals(const TVector& V, T Tolerance=KINDA_SMALL_NUMBER) const;
/**
* Checks whether all components of this vector are the same, within a tolerance.
*
* @param Tolerance Error tolerance.
* @return true if the vectors are equal within tolerance limits, false otherwise.
*/
bool AllComponentsEqual(T Tolerance=KINDA_SMALL_NUMBER) const;
/**
* Get a negated copy of the vector.
*
* @return A negated copy of the vector.
*/
FORCEINLINE TVector operator-() const;
/**
* Adds another vector to this.
* Uses component-wise addition.
*
* @param V Vector to add to this.
* @return Copy of the vector after addition.
*/
FORCEINLINE TVector operator+=(const TVector& V);
/**
* Subtracts another vector from this.
* Uses component-wise subtraction.
*
* @param V Vector to subtract from this.
* @return Copy of the vector after subtraction.
*/
FORCEINLINE TVector operator-=(const TVector& V);
/**
* Scales the vector.
*
* @param Scale Amount to scale this vector by.
* @return Copy of the vector after scaling.
*/
template::value)>
FORCEINLINE TVector operator*=(FArg Scale)
{
X *= Scale; Y *= Scale; Z *= Scale;
DiagnosticCheckNaN();
return *this;
}
/**
* Divides the vector by a number.
*
* @param V What to divide this vector by.
* @return Copy of the vector after division.
*/
template::value)>
TVector operator/=(FArg Scale)
{
const T RV = (T)1 / Scale;
X *= RV; Y *= RV; Z *= RV;
DiagnosticCheckNaN();
return *this;
}
/**
* Multiplies the vector with another vector, using component-wise multiplication.
*
* @param V What to multiply this vector with.
* @return Copy of the vector after multiplication.
*/
TVector operator*=(const TVector& V);
/**
* Divides the vector by another vector, using component-wise division.
*
* @param V What to divide vector by.
* @return Copy of the vector after division.
*/
TVector operator/=(const TVector& V);
/**
* Gets specific component of the vector.
*
* @param Index the index of vector component
* @return reference to component.
*/
T& operator[](int32 Index);
/**
* Gets specific component of the vector.
*
* @param Index the index of vector component
* @return Copy of the component.
*/
T operator[](int32 Index)const;
/**
* Gets a specific component of the vector.
*
* @param Index The index of the component required.
*
* @return Reference to the specified component.
*/
T& Component(int32 Index);
/**
* Gets a specific component of the vector.
*
* @param Index The index of the component required.
* @return Copy of the specified component.
*/
T Component(int32 Index) const;
/** Get a specific component of the vector, given a specific axis by enum */
T GetComponentForAxis(EAxis::Type Axis) const;
/** Set a specified componet of the vector, given a specific axis by enum */
void SetComponentForAxis(EAxis::Type Axis, T Component);
public:
// Simple functions.
/**
* Set the values of the vector directly.
*
* @param InX New X coordinate.
* @param InY New Y coordinate.
* @param InZ New Z coordinate.
*/
void Set(T InX, T InY, T InZ);
/**
* Get the maximum value of the vector's components.
*
* @return The maximum value of the vector's components.
*/
T GetMax() const;
/**
* Get the maximum absolute value of the vector's components.
*
* @return The maximum absolute value of the vector's components.
*/
T GetAbsMax() const;
/**
* Get the minimum value of the vector's components.
*
* @return The minimum value of the vector's components.
*/
T GetMin() const;
/**
* Get the minimum absolute value of the vector's components.
*
* @return The minimum absolute value of the vector's components.
*/
T GetAbsMin() const;
/** Gets the component-wise min of two vectors. */
TVector ComponentMin(const TVector& Other) const;
/** Gets the component-wise max of two vectors. */
TVector ComponentMax(const TVector& Other) const;
/**
* Get a copy of this vector with absolute value of each component.
*
* @return A copy of this vector with absolute value of each component.
*/
TVector GetAbs() const;
/**
* Get the length (magnitude) of this vector.
*
* @return The length of this vector.
*/
T Size() const;
/**
* Get the length (magnitude) of this vector.
*
* @return The length of this vector.
*/
T Length() const;
/**
* Get the squared length of this vector.
*
* @return The squared length of this vector.
*/
T SizeSquared() const;
/**
* Get the squared length of this vector.
*
* @return The squared length of this vector.
*/
T SquaredLength() const;
/**
* Get the length of the 2D components of this vector.
*
* @return The 2D length of this vector.
*/
T Size2D() const ;
/**
* Get the squared length of the 2D components of this vector.
*
* @return The squared 2D length of this vector.
*/
T SizeSquared2D() const ;
/**
* Checks whether vector is near to zero within a specified tolerance.
*
* @param Tolerance Error tolerance.
* @return true if the vector is near to zero, false otherwise.
*/
bool IsNearlyZero(T Tolerance=KINDA_SMALL_NUMBER) const;
/**
* Checks whether all components of the vector are exactly zero.
*
* @return true if the vector is exactly zero, false otherwise.
*/
bool IsZero() const;
/**
* Check if the vector is of unit length, with specified tolerance.
*
* @param LengthSquaredTolerance Tolerance against squared length.
* @return true if the vector is a unit vector within the specified tolerance.
*/
FORCEINLINE bool IsUnit(T LengthSquaredTolerance = KINDA_SMALL_NUMBER) const;
/**
* Checks whether vector is normalized.
*
* @return true if normalized, false otherwise.
*/
bool IsNormalized() const;
/**
* Normalize this vector in-place if it is larger than a given tolerance. Leaves it unchanged if not.
*
* @param Tolerance Minimum squared length of vector for normalization.
* @return true if the vector was normalized correctly, false otherwise.
*/
bool Normalize(T Tolerance=SMALL_NUMBER);
/**
* Calculates normalized version of vector without checking for zero length.
*
* @return Normalized version of vector.
* @see GetSafeNormal()
*/
FORCEINLINE TVector GetUnsafeNormal() const;
/**
* Gets a normalized copy of the vector, checking it is safe to do so based on the length.
* Returns zero vector by default if vector length is too small to safely normalize.
*
* @param Tolerance Minimum squared vector length.
* @return A normalized copy if safe, ResultIfZero otherwise.
*/
TVector GetSafeNormal(T Tolerance=SMALL_NUMBER, const TVector& ResultIfZero = ZeroVector) const;
/**
* Gets a normalized copy of the 2D components of the vector, checking it is safe to do so. Z is set to zero.
* Returns zero vector by default if vector length is too small to normalize.
*
* @param Tolerance Minimum squared vector length.
* @return Normalized copy if safe, otherwise returns ResultIfZero.
*/
TVector GetSafeNormal2D(T Tolerance=SMALL_NUMBER, const TVector& ResultIfZero = ZeroVector) const;
/**
* Util to convert this vector into a unit direction vector and its original length.
*
* @param OutDir Reference passed in to store unit direction vector.
* @param OutLength Reference passed in to store length of the vector.
*/
void ToDirectionAndLength(TVector& OutDir, double& OutLength) const;
void ToDirectionAndLength(TVector& OutDir, float& OutLength) const;
/**
* Get a copy of the vector as sign only.
* Each component is set to +1 or -1, with the sign of zero treated as +1.
*
* @param A copy of the vector with each component set to +1 or -1
*/
FORCEINLINE TVector GetSignVector() const;
/**
* Projects 2D components of vector based on Z.
*
* @return Projected version of vector based on Z.
*/
TVector Projection() const;
/**
* Calculates normalized 2D version of vector without checking for zero length.
*
* @return Normalized version of vector.
* @see GetSafeNormal2D()
*/
FORCEINLINE TVector GetUnsafeNormal2D() const;
/**
* Gets a copy of this vector snapped to a grid.
*
* @param GridSz Grid dimension.
* @return A copy of this vector snapped to a grid.
* @see FMath::GridSnap()
*/
TVector GridSnap(const T& GridSz) const;
/**
* Get a copy of this vector, clamped inside of a cube.
*
* @param Radius Half size of the cube.
* @return A copy of this vector, bound by cube.
*/
TVector BoundToCube(T Radius) const;
/** Get a copy of this vector, clamped inside of a cube. */
TVector BoundToBox(const TVector& Min, const TVector Max) const;
/** Create a copy of this vector, with its magnitude clamped between Min and Max. */
TVector GetClampedToSize(T Min, T Max) const;
/** Create a copy of this vector, with the 2D magnitude clamped between Min and Max. Z is unchanged. */
TVector GetClampedToSize2D(T Min, T Max) const;
/** Create a copy of this vector, with its maximum magnitude clamped to MaxSize. */
TVector GetClampedToMaxSize(T MaxSize) const;
/** Create a copy of this vector, with the maximum 2D magnitude clamped to MaxSize. Z is unchanged. */
TVector GetClampedToMaxSize2D(T MaxSize) const;
/**
* Add a vector to this and clamp the result in a cube.
*
* @param V Vector to add.
* @param Radius Half size of the cube.
*/
void AddBounded(const TVector& V, T Radius=MAX_int16);
/**
* Gets the reciprocal of this vector, avoiding division by zero.
* Zero components are set to BIG_NUMBER.
*
* @return Reciprocal of this vector.
*/
TVector Reciprocal() const;
/**
* Check whether X, Y and Z are nearly equal.
*
* @param Tolerance Specified Tolerance.
* @return true if X == Y == Z within the specified tolerance.
*/
bool IsUniform(T Tolerance=KINDA_SMALL_NUMBER) const;
/**
* Mirror a vector about a normal vector.
*
* @param MirrorNormal Normal vector to mirror about.
* @return Mirrored vector.
*/
TVector MirrorByVector(const TVector& MirrorNormal) const;
/**
* Mirrors a vector about a plane.
*
* @param Plane Plane to mirror about.
* @return Mirrored vector.
*/
TVector MirrorByPlane(const TPlane& Plane) const;
/**
* Rotates around Axis (assumes Axis.Size() == 1).
*
* @param Angle Angle to rotate (in degrees).
* @param Axis Axis to rotate around.
* @return Rotated Vector.
*/
TVector RotateAngleAxis(const T AngleDeg, const TVector& Axis) const;
/**
* Returns the cosine of the angle between this vector and another projected onto the XY plane (no Z).
*
* @param B the other vector to find the 2D cosine of the angle with.
* @return The cosine.
*/
FORCEINLINE T CosineAngle2D(TVector B) const;
/**
* Gets a copy of this vector projected onto the input vector.
*
* @param A Vector to project onto, does not assume it is normalized.
* @return Projected vector.
*/
FORCEINLINE TVector ProjectOnTo(const TVector& A) const ;
/**
* Gets a copy of this vector projected onto the input vector, which is assumed to be unit length.
*
* @param Normal Vector to project onto (assumed to be unit length).
* @return Projected vector.
*/
FORCEINLINE TVector ProjectOnToNormal(const TVector& Normal) const;
/**
* Return the TRotator orientation corresponding to the direction in which the vector points.
* Sets Yaw and Pitch to the proper numbers, and sets Roll to zero because the roll can'T be determined from a vector.
*
* @return TRotator from the Vector's direction, without any roll.
* @see ToOrientationQuat()
*/
CORE_API TRotator ToOrientationRotator() const;
/**
* Return the Quaternion orientation corresponding to the direction in which the vector points.
* Similar to the UE::Math::TRotator version, returns a result without roll such that it preserves the up vector.
*
* @note If you don'T care about preserving the up vector and just want the most direct rotation, you can use the faster
* 'FQuat::FindBetweenVectors(FVector::ForwardVector, YourVector)' or 'FQuat::FindBetweenNormals(...)' if you know the vector is of unit length.
*
* @return Quaternion from the Vector's direction, without any roll.
* @see ToOrientationRotator(), FQuat::FindBetweenVectors()
*/
CORE_API TQuat ToOrientationQuat() const;
/**
* Return the UE::Math::TRotator orientation corresponding to the direction in which the vector points.
* Sets Yaw and Pitch to the proper numbers, and sets Roll to zero because the roll can't be determined from a vector.
* @note Identical to 'ToOrientationRotator()' and preserved for legacy reasons.
* @return UE::Math::TRotator from the Vector's direction.
* @see ToOrientationRotator(), ToOrientationQuat()
*/
FORCEINLINE UE::Math::TRotator Rotation() const
{
return ToOrientationRotator();
}
/**
* Find good arbitrary axis vectors to represent U and V axes of a plane,
* using this vector as the normal of the plane.
*
* @param Axis1 Reference to first axis.
* @param Axis2 Reference to second axis.
*/
void FindBestAxisVectors(TVector& Axis1, TVector& Axis2) const;
/** When this vector contains Euler angles (degrees), ensure that angles are between +/-180 */
void UnwindEuler();
/**
* Utility to check if there are any non-finite values (NaN or Inf) in this vector.
*
* @return true if there are any non-finite values in this vector, false otherwise.
*/
bool ContainsNaN() const;
/**
* Get a textual representation of this vector.
*
* @return A string describing the vector.
*/
FString ToString() const;
/**
* Get a locale aware textual representation of this vector.
*
* @return A string describing the vector.
*/
FText ToText() const;
/** Get a short textural representation of this vector, for compact readable logging. */
FString ToCompactString() const;
/** Get a short locale aware textural representation of this vector, for compact readable logging. */
FText ToCompactText() const;
/**
* Initialize this Vector based on an FString. The String is expected to contain X=, Y=, Z=.
* The TVector will be bogus when InitFromString returns false.
*
* @param InSourceString FString containing the vector values.
* @return true if the X,Y,Z values were read successfully; false otherwise.
*/
bool InitFromString(const FString& InSourceString);
/**
* Initialize this Vector based on an FString. The String is expected to contain V(0)
* or at least one value X=, Y=, Z=, previously produced by ToCompactString()
* The TVector will be bogus when InitFromString returns false.
*
* @param InSourceString FString containing the vector values.
* @return true if any of the X,Y,Z values were read successfully; false otherwise.
*/
bool InitFromCompactString(const FString& InSourceString);
/**
* Converts a Cartesian unit vector into spherical coordinates on the unit sphere.
* @return Output Theta will be in the range [0, PI], and output Phi will be in the range [-PI, PI].
*/
TVector2 UnitCartesianToSpherical() const;
/**
* Convert a direction vector into a 'heading' angle.
*
* @return 'Heading' angle between +/-PI. 0 is pointing down +X.
*/
T HeadingAngle() const;
/**
* Create an orthonormal basis from a basis with at least two orthogonal vectors.
* It may change the directions of the X and Y axes to make the basis orthogonal,
* but it won'T change the direction of the Z axis.
* All axes will be normalized.
*
* @param XAxis The input basis' XAxis, and upon return the orthonormal basis' XAxis.
* @param YAxis The input basis' YAxis, and upon return the orthonormal basis' YAxis.
* @param ZAxis The input basis' ZAxis, and upon return the orthonormal basis' ZAxis.
*/
static void CreateOrthonormalBasis(TVector& XAxis,TVector& YAxis,TVector& ZAxis);
/**
* Compare two points and see if they're the same, using a threshold.
*
* @param P First vector.
* @param Q Second vector.
* @return Whether points are the same within a threshold. Uses fast distance approximation (linear per-component distance).
*/
static bool PointsAreSame(const TVector &P, const TVector &Q);
/**
* Compare two points and see if they're within specified distance.
*
* @param Point1 First vector.
* @param Point2 Second vector.
* @param Dist Specified distance.
* @return Whether two points are within the specified distance. Uses fast distance approximation (linear per-component distance).
*/
static bool PointsAreNear(const TVector &Point1, const TVector &Point2, T Dist);
/**
* Calculate the signed distance (in the direction of the normal) between a point and a plane.
*
* @param Point The Point we are checking.
* @param PlaneBase The Base Point in the plane.
* @param PlaneNormal The Normal of the plane (assumed to be unit length).
* @return Signed distance between point and plane.
*/
static T PointPlaneDist(const TVector &Point, const TVector &PlaneBase, const TVector &PlaneNormal);
/**
* Calculate the projection of a point on the given plane.
*
* @param Point The point to project onto the plane
* @param Plane The plane
* @return Projection of Point onto Plane
*/
static TVector PointPlaneProject(const TVector& Point, const TPlane& Plane);
/**
* Calculate the projection of a point on the plane defined by counter-clockwise (CCW) points A,B,C.
*
* @param Point The point to project onto the plane
* @param A 1st of three points in CCW order defining the plane
* @param B 2nd of three points in CCW order defining the plane
* @param C 3rd of three points in CCW order defining the plane
* @return Projection of Point onto plane ABC
*/
static TVector PointPlaneProject(const TVector& Point, const TVector& A, const TVector& B, const TVector& C);
/**
* Calculate the projection of a point on the plane defined by PlaneBase and PlaneNormal.
*
* @param Point The point to project onto the plane
* @param PlaneBase Point on the plane
* @param PlaneNorm Normal of the plane (assumed to be unit length).
* @return Projection of Point onto plane
*/
static TVector PointPlaneProject(const TVector& Point, const TVector& PlaneBase, const TVector& PlaneNormal);
/**
* Calculate the projection of a vector on the plane defined by PlaneNormal.
*
* @param V The vector to project onto the plane.
* @param PlaneNormal Normal of the plane (assumed to be unit length).
* @return Projection of V onto plane.
*/
static TVector VectorPlaneProject(const TVector& V, const TVector& PlaneNormal);
/**
* Euclidean distance between two points.
*
* @param V1 The first point.
* @param V2 The second point.
* @return The distance between two points.
*/
static FORCEINLINE T Dist(const TVector &V1, const TVector &V2);
static FORCEINLINE T Distance(const TVector &V1, const TVector &V2) { return Dist(V1, V2); }
/**
* Euclidean distance between two points in the XY plane (ignoring Z).
*
* @param V1 The first point.
* @param V2 The second point.
* @return The distance between two points in the XY plane.
*/
static FORCEINLINE T DistXY(const TVector &V1, const TVector &V2);
static FORCEINLINE T Dist2D(const TVector &V1, const TVector &V2) { return DistXY(V1, V2); }
/**
* Squared distance between two points.
*
* @param V1 The first point.
* @param V2 The second point.
* @return The squared distance between two points.
*/
static FORCEINLINE T DistSquared(const TVector &V1, const TVector &V2);
/**
* Squared distance between two points in the XY plane only.
*
* @param V1 The first point.
* @param V2 The second point.
* @return The squared distance between two points in the XY plane
*/
static FORCEINLINE T DistSquaredXY(const TVector &V1, const TVector &V2);
static FORCEINLINE T DistSquared2D(const TVector &V1, const TVector &V2) { return DistSquaredXY(V1, V2); }
/**
* Compute pushout of a box from a plane.
*
* @param Normal The plane normal.
* @param Size The size of the box.
* @return Pushout required.
*/
static FORCEINLINE T BoxPushOut(const TVector& Normal, const TVector& Size);
/**
* Min, Max, Min3, Max3 like FMath
*/
static FORCEINLINE TVector Min( const TVector& A, const TVector& B );
static FORCEINLINE TVector Max( const TVector& A, const TVector& B );
static FORCEINLINE TVector Min3( const TVector& A, const TVector& B, const TVector& C );
static FORCEINLINE TVector Max3( const TVector& A, const TVector& B, const TVector& C );
/**
* See if two normal vectors are nearly parallel, meaning the angle between them is close to 0 degrees.
*
* @param Normal1 First normalized vector.
* @param Normal1 Second normalized vector.
* @param ParallelCosineThreshold Normals are parallel if absolute value of dot product (cosine of angle between them) is greater than or equal to this. For example: cos(1.0 degrees).
* @return true if vectors are nearly parallel, false otherwise.
*/
static bool Parallel(const TVector& Normal1, const TVector& Normal2, T ParallelCosineThreshold = THRESH_NORMALS_ARE_PARALLEL);
/**
* See if two normal vectors are coincident (nearly parallel and point in the same direction).
*
* @param Normal1 First normalized vector.
* @param Normal2 Second normalized vector.
* @param ParallelCosineThreshold Normals are coincident if dot product (cosine of angle between them) is greater than or equal to this. For example: cos(1.0 degrees).
* @return true if vectors are coincident (nearly parallel and point in the same direction), false otherwise.
*/
static bool Coincident(const TVector& Normal1, const TVector& Normal2, T ParallelCosineThreshold = THRESH_NORMALS_ARE_PARALLEL);
/**
* See if two normal vectors are nearly orthogonal (perpendicular), meaning the angle between them is close to 90 degrees.
*
* @param Normal1 First normalized vector.
* @param Normal2 Second normalized vector.
* @param OrthogonalCosineThreshold Normals are orthogonal if absolute value of dot product (cosine of angle between them) is less than or equal to this. For example: cos(89.0 degrees).
* @return true if vectors are orthogonal (perpendicular), false otherwise.
*/
static bool Orthogonal(const TVector& Normal1, const TVector& Normal2, T OrthogonalCosineThreshold = THRESH_NORMALS_ARE_ORTHOGONAL);
/**
* See if two planes are coplanar. They are coplanar if the normals are nearly parallel and the planes include the same set of points.
*
* @param Base1 The base point in the first plane.
* @param Normal1 The normal of the first plane.
* @param Base2 The base point in the second plane.
* @param Normal2 The normal of the second plane.
* @param ParallelCosineThreshold Normals are parallel if absolute value of dot product is greater than or equal to this.
* @return true if the planes are coplanar, false otherwise.
*/
static bool Coplanar(const TVector& Base1, const TVector& Normal1, const TVector& Base2, const TVector& Normal2, T ParallelCosineThreshold = THRESH_NORMALS_ARE_PARALLEL);
/**
* Triple product of three vectors: X dot (Y cross Z).
*
* @param X The first vector.
* @param Y The second vector.
* @param Z The third vector.
* @return The triple product: X dot (Y cross Z).
*/
static T Triple(const TVector& X, const TVector& Y, const TVector& Z);
/**
* Generates a list of sample points on a Bezier curve defined by 2 points.
*
* @param ControlPoints Array of 4 FVectors (vert1, controlpoint1, controlpoint2, vert2).
* @param NumPoints Number of samples.
* @param OutPoints Receives the output samples.
* @return The path length.
*/
static T EvaluateBezier(const TVector* ControlPoints, int32 NumPoints, TArray>& OutPoints);
/**
* Converts a vector containing radian values to a vector containing degree values.
*
* @param RadVector Vector containing radian values
* @return Vector containing degree values
*/
static TVector RadiansToDegrees(const TVector& RadVector);
/**
* Converts a vector containing degree values to a vector containing radian values.
*
* @param DegVector Vector containing degree values
* @return Vector containing radian values
*/
static TVector DegreesToRadians(const TVector& DegVector);
/**
* Given a current set of cluster centers, a set of points, iterate N times to move clusters to be central.
*
* @param Clusters Reference to array of Clusters.
* @param Points Set of points.
* @param NumIterations Number of iterations.
* @param NumConnectionsToBeValid Sometimes you will have long strings that come off the mass of points
* which happen to have been chosen as Cluster starting points. You want to be able to disregard those.
*/
static void GenerateClusterCenters(TArray>& Clusters, const TArray>& Points, int32 NumIterations, int32 NumConnectionsToBeValid);
bool Serialize(FStructuredArchive::FSlot Slot)
{
Slot << (TVector&)*this;
return true;
}
bool SerializeFromMismatchedTag(FName StructTag, FStructuredArchive::FSlot Slot);
/**
* Network serialization function.
* FVectors NetSerialize without quantization (ie exact values are serialized). se the FVectors_NetQuantize etc (NetSerialization.h) instead.
*
* @see FVector_NetQuantize, FVector_NetQuantize10, FVector_NetQuantize100, FVector_NetQuantizeNormal
*/
bool NetSerialize(FArchive& Ar, class UPackageMap* Map, bool& bOutSuccess)
{
if (Ar.EngineNetVer() >= HISTORY_SERIALIZE_DOUBLE_VECTORS_AS_DOUBLES)
{
Ar << X << Y << Z;
}
else
{
checkf(Ar.IsLoading(), TEXT("float -> double conversion applied outside of load!"));
// Always serialize as float
float SX, SY, SZ;
Ar << SX << SY << SZ;
X = SX;
Y = SY;
Z = SZ;
}
return true;
}
// Conversion from other type.
template::Value)>
explicit TVector(const TVector& From) : TVector((T)From.X, (T)From.Y, (T)From.Z) {}
};
/**
* Structured archive slot serializer for FVector3f.
*
* @param Slot Structured archive slot.
* @param V Vector to serialize.
*/
inline void operator<<(FStructuredArchive::FSlot Slot, TVector& V)
{
// @warning BulkSerialize: FVector3f is serialized as memory dump
// See TArray::BulkSerialize for detailed description of implied limitations.
FStructuredArchive::FRecord Record = Slot.EnterRecord();
Record << SA_VALUE(TEXT("X"), V.X);
Record << SA_VALUE(TEXT("Y"), V.Y);
Record << SA_VALUE(TEXT("Z"), V.Z);
V.DiagnosticCheckNaN();
}
/**
* Structured archive slot serializer for FVector3d.
*
* @param Slot Structured archive slot.
* @param V Vector to serialize.
*/
inline void operator<<(FStructuredArchive::FSlot Slot, TVector& V)
{
// @warning BulkSerialize: FVector3d is serialized as memory dump
// See TArray::BulkSerialize for detailed description of implied limitations.
FStructuredArchive::FRecord Record = Slot.EnterRecord();
if (Slot.GetUnderlyingArchive().UEVer() >= EUnrealEngineObjectUE5Version::LARGE_WORLD_COORDINATES)
{
Record << SA_VALUE(TEXT("X"), V.X);
Record << SA_VALUE(TEXT("Y"), V.Y);
Record << SA_VALUE(TEXT("Z"), V.Z);
}
else
{
checkf(Slot.GetUnderlyingArchive().IsLoading(), TEXT("float -> double conversion applied outside of load!"));
// Stored as floats, so serialize float and copy.
float X, Y, Z;
Record << SA_VALUE(TEXT("X"), X);
Record << SA_VALUE(TEXT("Y"), Y);
Record << SA_VALUE(TEXT("Z"), Z);
V = TVector(X, Y, Z);
}
V.DiagnosticCheckNaN();
}
/* FVector inline functions
*****************************************************************************/
template
FORCEINLINE TVector::TVector(const TVector2 V, T InZ)
: X(V.X), Y(V.Y), Z(InZ)
{
DiagnosticCheckNaN();
}
template
inline TVector TVector::RotateAngleAxis(const T AngleDeg, const TVector& Axis) const
{
T S, C;
FMath::SinCos(&S, &C, FMath::DegreesToRadians(AngleDeg));
const T XX = Axis.X * Axis.X;
const T YY = Axis.Y * Axis.Y;
const T ZZ = Axis.Z * Axis.Z;
const T XY = Axis.X * Axis.Y;
const T YZ = Axis.Y * Axis.Z;
const T ZX = Axis.Z * Axis.X;
const T XS = Axis.X * S;
const T YS = Axis.Y * S;
const T ZS = Axis.Z * S;
const T OMC = 1.f - C;
return TVector(
(OMC * XX + C) * X + (OMC * XY - ZS) * Y + (OMC * ZX + YS) * Z,
(OMC * XY + ZS) * X + (OMC * YY + C) * Y + (OMC * YZ - XS) * Z,
(OMC * ZX - YS) * X + (OMC * YZ + XS) * Y + (OMC * ZZ + C) * Z
);
}
template
inline bool TVector::PointsAreSame(const TVector& P, const TVector& Q)
{
T Temp;
Temp=P.X-Q.X;
if((Temp > -THRESH_POINTS_ARE_SAME) && (Temp < THRESH_POINTS_ARE_SAME))
{
Temp=P.Y-Q.Y;
if((Temp > -THRESH_POINTS_ARE_SAME) && (Temp < THRESH_POINTS_ARE_SAME))
{
Temp=P.Z-Q.Z;
if((Temp > -THRESH_POINTS_ARE_SAME) && (Temp < THRESH_POINTS_ARE_SAME))
{
return true;
}
}
}
return false;
}
template
inline bool TVector::PointsAreNear(const TVector& Point1, const TVector& Point2, T Dist)
{
T Temp;
Temp=(Point1.X - Point2.X); if (FMath::Abs(Temp)>=Dist) return false;
Temp=(Point1.Y - Point2.Y); if (FMath::Abs(Temp)>=Dist) return false;
Temp=(Point1.Z - Point2.Z); if (FMath::Abs(Temp)>=Dist) return false;
return true;
}
template
inline T TVector::PointPlaneDist
(
const TVector &Point,
const TVector &PlaneBase,
const TVector &PlaneNormal
)
{
return (Point - PlaneBase) | PlaneNormal;
}
template
inline TVector TVector::PointPlaneProject(const TVector& Point, const TVector& PlaneBase, const TVector& PlaneNorm)
{
//Find the distance of X from the plane
//Add the distance back along the normal from the point
return Point - TVector::PointPlaneDist(Point,PlaneBase,PlaneNorm) * PlaneNorm;
}
template
inline TVector TVector::VectorPlaneProject(const TVector& V, const TVector& PlaneNormal)
{
return V - V.ProjectOnToNormal(PlaneNormal);
}
template
inline bool TVector::Parallel(const TVector
