Fixed several recursive header issues, refactored Math lib, began implementing core mathematical functions as wrappers around stdmath, will implement SSE and lookup tables later.
All checks were successful
Run ReCI Build Test / Explore-Gitea-Actions (push) Successful in 1m13s
Build Docs With Doxygen / Explore-Gitea-Actions (push) Successful in 19s

This commit is contained in:
2024-07-05 16:13:13 -04:00
parent bc7adae8af
commit 9253cfc8c7
27 changed files with 1217 additions and 335 deletions

View File

@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.18)
cmake_minimum_required(VERSION 3.18...3.28)
PROJECT(J3ML
VERSION 1.1
LANGUAGES CXX

View File

@@ -0,0 +1,3 @@
#pragma once

View File

@@ -20,4 +20,6 @@
#include <J3ML/Geometry/Triangle2D.h>
#include <J3ML/Geometry/TriangleMesh.h>
#include <J3ML/Geometry/KDTree.h>
using namespace J3ML::Geometry;

View File

@@ -1,15 +1,27 @@
#pragma once
#include <format>
#include <optional>
#include <J3ML/LinearAlgebra.h>
#include <J3ML/LinearAlgebra/Common.h>
#include <J3ML/Geometry/Common.h>
#include <J3ML/Geometry/Shape.h>
#include "J3ML/Algorithm/RNG.h"
#include "Polygon.h"
#include "Sphere.h"
#include <J3ML/Algorithm/RNG.h>
namespace J3ML::Geometry
{
// TODO: Move this somewhere else to do goofy experiments!
template <typename T, typename = std::enable_if_t<std::is_arithmetic_v<T>, T>>
float fdiv(T a, T b) {
return (float)a/(b);
}
using namespace J3ML::LinearAlgebra;
using J3ML::Algorithm::RNG;
@@ -47,12 +59,25 @@ namespace J3ML::Geometry
@note Since an AABB cannot generally represent an OBB, this conversion is not exact, but the returned AABB
specifies a larger volume.
@see class OBB. */
explicit AABB(const OBB &obb);
explicit AABB(const OBB &obb) {
SetFrom(obb);
}
/// Constructs this AABB to enclose the given Sphere.
/** @see class Sphere. */
explicit AABB(const Sphere &s);
explicit AABB(const Sphere &s) {
SetFrom(s);
}
/// Returns the diameter vector of this AABB.
/** @note For AABB, Diagonal() and Size() are the same concept. These functions are provided for symmetry with the OBB class.
@see Size(), HalfDiagonal() */
Vector3 Diagonal() const { return Size(); }
/// Returns Diagonal()/2.
/// @see Diagonal(), HalfSize()
Vector3 HalfDiagonal() const { return HalfSize(); }
static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
@@ -184,6 +209,7 @@ namespace J3ML::Geometry
float SurfaceArea() const;
Vector3 GetClosestPoint(const Vector3& point) const;
void Translate(const Vector3& offset);
AABB Translated(const Vector3& offset) const;
void Scale(const Vector3& scale);
@@ -329,12 +355,27 @@ namespace J3ML::Geometry
void Enclose(const LineSegment &lineSegment);
void Enclose(const OBB &obb);
void Enclose(const Sphere &sphere);
void Enclose(const Triangle &triangle);
void Enclose(const Capsule &capsule);
void Enclose(const Frustum &frustum);
void Enclose(const Polygon &polygon);
void Enclose(const Polyhedron &polyhedron);
void Enclose(const Vector3 *pointArray, int numPoints);
void Enclose(const Frustum &frustum) {
//Enclose(frustum.MinimalEnclosingAABB());
}
void Enclose(const Polygon &polygon) {
//Enclose(polygon.MinimalEnclosingAABB());
}
void Enclose(const Polyhedron &polyhedron) {
//Enclose(polyhedron.MinimalEnclosingAABB());
}
void Enclose(const Vector3 *pointArray, int numPoints) {
assert(pointArray || numPoints == 0);
if (!pointArray)
return;
for (int i = 0; i < numPoints; ++i)
Enclose(pointArray[i]);
}
bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;

View File

@@ -1,9 +1,9 @@
#pragma once
#include "LineSegment.h"
#include "Shape.h"
#include <J3ML/LinearAlgebra.h>
#include <J3ML/LinearAlgebra/Common.h>
#include <J3ML/Geometry/Common.h>
#include <J3ML/Geometry/Shape.h>
namespace J3ML::Geometry
{
@@ -63,12 +63,16 @@ namespace J3ML::Geometry
/// Tests if this Capsule is degenerate.
/** @return True if this Capsule does not span a strictly positive volume. */
bool IsDegenerate() const;
/// Computes the total height of this capsule, i.e. LineLength() + Diameter().
/** <img src="CapsuleFunctions.png" />
@see LineLength(). */
float Height() const;
/// Computes the distance of the two inner points of this capsule. @see Height.
float LineLength() const { return l.Length(); }
/// Computes the diameter of this capsule.
float Diameter() const;
float Diameter() const { return 2.f * r;}
/// Returns the bottom-most point of this Capsule.
/** <img src="CapsuleFunctions.png" />
@note The bottom-most point is only a naming convention, and does not correspond to the bottom-most point along any world axis. The returned
@@ -76,12 +80,13 @@ namespace J3ML::Geometry
@note The bottom-most point of the capsule is different than the point l.a. The returned point is the point at the very far
edge of this capsule, and does not lie on the internal line. See the attached diagram.
@see Top(), l. */
Vector3 Bottom() const;
Vector3 Bottom() const { return l.A - UpDirection() * r;}
/// Returns the center point of this Capsule.
/** <img src="doc/static/docs/CapsuleFunctions.png" />
@return The point (l.a + l.b) / 2. This point is the center of mass for this capsule.
@see l, Bottom(), Top(). */
Vector3 Center() const;
Vector3 Center() const { return l.CenterPoint();}
Vector3 Centroid() const; ///< [similarOverload: Center]
/// Returns the direction from the bottommost point towards the topmost point of this Capsule.

View File

@@ -36,4 +36,6 @@ namespace J3ML::Geometry
bool operator==(const Interval& rhs) const = default;
};
}
}
using namespace J3ML::Geometry;

View File

@@ -333,13 +333,28 @@ namespace J3ML::Geometry
@see ToPolyhedron(). */
//PBVolume<6> ToPBVolume() const;
Matrix4x4 ViewProjMatrix() const { return viewProjectionMatrix; }
Matrix4x4 ComputeViewProjMatrix() const;
Vector3 Project(const Vector3& point) const {
Vector4 projectedPoint = ViewProjMatrix().Mul(Vector4(point, 1.f));
projectedPoint.NormalizeW();
return projectedPoint.XYZ();
}
/// Tests if the given object is fully contained inside this Frustum.
/** This function returns true if the given object lies inside this Frustum, and false otherwise.
@note The comparison is performed using less-or-equal, so the faces of this Frustum count as being inside, but
due to float inaccuracies, this cannot generally be relied upon.
@todo Add Contains(Circle/Disc/Sphere/Capsule).
@see Distance(), Intersects(), ClosestPoint(). */
bool Contains(const Vector3 &point) const;
bool Contains(const Vector3 &point) const {
const float eps = 1e-3f;
const float position = 1.f + eps;
const float neg = -position;
Vector3 projected = Project(point);
}
bool Contains(const LineSegment &lineSegment) const;
bool Contains(const Triangle &triangle) const;
bool Contains(const Polygon &polygon) const;

View File

@@ -1,12 +1,83 @@
#pragma once
namespace J3ML::Geometry
{
enum CardinalAxis
{
AxisX = 0,
AxisY,
AxisZ,
AxisNone
};
struct KdTreeNode
{
/// If this is an inner node, specifies along which axis this node is split.
/// If this is a leaf, has the value AxisNone.
unsigned splitAxis : 2;
/// If this is an inner node, specifies the index/offset to the child node pair.
/// If this is a leaf, the value is undefined.
unsigned childIndex : 30;
union
{
/// If this is an inner node, specifies the position along the cardinal axis of the split.
float splitPos;
/// If this is a leaf, specifies the index/ofset to the object bucket for this leaf.
/// If zero, this leaf does not have a bucket associated with it. (empty leaf)
u32 bucketIndex;
};
/// If true, this leaf does not contain any objects.
bool IsEmptyLeaf() const { assert(IsLeaf()); return bucketIndex == 0; }
bool IsLeaf() const { return splitAxis == AxisNone; }
int LeftChildIndex() const { return (int)childIndex; }
int RightChildIndex() const { return (int) childIndex+1; }
CardinalAxis SplitAxis() const { return (CardinalAxis) splitAxis;}
};
/// A KD-tree accelleration structure for static geometry.
template <typename T>
class KdTree
{
public:
KdTree() {}
//~KDTree() { /* TODO: FILL */}
void AddObjects(const T *objects, int numObjects);
void Build();
void Clear();
u32* Bucket(int bucketIndex);
const u32* Bucket(int bucketIndex) const;
T& Object(int objectIndex);
const T& Object(int objectIndex) const;
int NumObjects() const;
int NumNodes() const;
int NumLeaves() const;
int NumInnerNodes() const;
int TreeHeight() const;
KdTreeNode* Root();
const KdTreeNode* Root() const;
bool IsPartOfThisTree(const KdTreeNode *node) const;
//const AABB& BoundingAABB() const { return rootAABB;}
/// Traverses a ray through this kD-tree, and calls the given leafCallback function for each leaf of the tree.
/// Uses the "recursive B" method from Vlastimil Havran's thesis.
template <typename Func>
void RayQuery(const Ray& r, Func &leaftCallback);
template <typename Func>
void AABBQuery(const AABB& aabb, Func& leafCallback);
private:
static const int maxNodes = 256 * 1024;
static const int maxTreeDepth = 30;
std::vector<KdTreeNode> nodes;
//std::vector<u8, Aligned
};
}

View File

@@ -1,48 +1,53 @@
#pragma once
#include <J3ML/LinearAlgebra/Vector3.h>
#include "Plane.h"
#include <J3ML/LinearAlgebra/Common.h>
#include <J3ML/Geometry/Common.h>
#include <J3ML/LinearAlgebra/Vector3.h>
namespace J3ML::Geometry
{
/// Clamps the given input value to the range [min, max].
/** @see Clamp01(), Min(), Max(). */
template<typename T>
T Clamp(const T &val, const T &floor, const T &ceil)
{
assert(floor <= ceil);
return val <= ceil ? (val >= floor ? val : floor) : ceil;
}
/// Clamps the given input value to the range [0, 1].
/** @see Clamp(), Min(), Max(). */
template<typename T>
T Clamp01(const T &val) { return Clamp(val, T(0), T(1)); }
using LinearAlgebra::Vector3;
/// A line segment in 3D space is a finite line with a start and end point.
class LineSegment
{
public:
Vector3 A;
Vector3 B;
Vector3 A; /// The starting point of this line segment.
Vector3 B; /// The end point of this line segment.
public:
/// The default constructor does not initialize any members of this class.
/** This means that the values of the members A and B are undefined after creating a new LineSegment using this
default constructor. Remember to assign to them before use.
@see A, B. */
LineSegment();
/// Constructs a line segment through the given end points.
/** @see a, b. */
LineSegment(const Vector3& a, const Vector3& b);
/// Computes the closest point on this line segment to the given object.
/** @param d [out] If specified, this parameter receives the normalized distance along
this line segment which specifies the closest point on this line segment to
the specified point.
@return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Vector3 &point) const;
bool Contains(const Vector3& point, float distanceThreshold = 1e-3f) const;
/// Constructs a line segment from a ray or a line.
/** This constructor takes the ray/line origin position as the starting point of this line segment, and defines the end point
of the line segment using the given distance parameter.
@param d The distance along the ray/line for the end point of this line segment. This will set b = ray.pos + d * ray.dir
as the end point. When converting a ray to a line segment, it is possible to pass in a d value < 0, but in that case
the resulting line segment will not lie on the ray.
@see a, b, classes Ray, Line, Line::GetPoint(), Ray::GetPoint() */
explicit LineSegment(const Ray &ray, float d);
explicit LineSegment(const Line &line, float d);
/// Tests if the given point or line segment is contained on this line segment.
/** @param distanceThreshold Because a line segment is an one-dimensional object in 3D space, an epsilon value
is used as a threshold for this test. This effectively transforms this line segment to a capsule with
the radius indicated by this value.
@return True if this line segment contains the given point or line segment.
@see Intersects, ClosestPoint(), Distance(). */
bool Contains(const Vector3& point, float distanceThreshold = 1e-3f) const;
bool Contains(const LineSegment &lineSegment, float distanceThreshold = 1e-3f) const;
/// Quickly returns an arbitrary point inside this LineSegment. Used in GJK intersection test.
inline Vector3 AnyPointFast() const { return A; }
Vector3 AnyPointFast() const;
/// Computes an extreme point of this LineSegment in the given direction.
/** An extreme point is a farthest point along this LineSegment in the given direction. Given a direction,
@@ -52,40 +57,189 @@ namespace J3ML::Geometry
@return An extreme point of this LineSegment in the given direction. The returned point is always
either a or b.
@see a, b.*/
Vector3 ExtremePoint(const Vector3 &direction) const;
[[nodiscard]] Vector3 ExtremePoint(const Vector3 &direction) const;
Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
/// Translates this LineSegment in world space.
/** @param offset The amount of displacement to apply to this LineSegment, in world space coordinates.
@see Transform(). */
void Translate(const Vector3 &offset);
/// Applies a transformation to this line.
/** This function operates in-place.
@see Translate(), classes Matrix3x3, Matrix4x4, Quaternion, Transform(). */
void Transform(const Matrix3x3 &transform);
void Transform(const Matrix4x4 &transform);
void Transform(const Quaternion &transform);
void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
Vector3 GetPoint(float d) const;
Vector3 ClosestPoint(const Vector3 &point, float &d) const;
Vector3 ClosestPoint(const Ray &other, float &d, float &d2) const;
float Distance(const Vector3 &point) const;
float DistanceSq(const Vector3& point) const;
float Distance(const Vector3 &point, float &d) const;
float Distance(const Ray &other) const;
float Distance(const Ray &other, float &d) const;
float Distance(const Ray &other, float &d, float &d2) const;
float Distance(const Line &other) const;
float Distance(const Line &other, float &d) const;
float Distance(const Line &other, float &d, float &d2) const;
float Distance(const LineSegment &other) const;
float Distance(const LineSegment &other, float &d) const;
float Distance(const LineSegment &other, float &d, float &d2) const;
float Distance(const Plane& other) const;
Vector3 Dir() const;
/// Returns a point on the line.
/** @param d The normalized distance along the line segment to compute. If a value in the range [0, 1] is passed, then the
returned point lies along this line segment. If some other value is specified, the returned point lies on the
line defined by this line segment, but not inside the interval from a to b.
@note The meaning of d here differs from Line::GetPoint and Ray::GetPoint. For the class LineSegment,
GetPoint(0) returns a, and GetPoint(1) returns b. This means that GetPoint(1) will not generally be exactly one unit
away from the starting point of this line segment, as is the case with Line and Ray.
@return (1-d)*a + d*b.
@see a, b, Line::GetPoint(), Ray::GetPoint(). */
[[nodiscard]] Vector3 GetPoint(float d) const;
float Length() const;
/// Returns the center point of this line segment.
/** This function is the same as calling GetPoint(0.5f), but provided here as conveniency.
@see GetPoint(). */
[[nodiscard]] Vector3 CenterPoint() const;
float LengthSq() const;
/// Reverses the direction of this line segment.
/** This function swaps the start and end points of this line segment so that it runs from b to a.
This does not have an effect on the set of points represented by this line segment, but it reverses
the direction of the vector returned by Dir().
@note This function operates in-place.
@see a, b, Dir(). */
void Reverse();
float DistanceSq(const LineSegment &other) const;
Vector3 ClosestPoint(const LineSegment &other, float &d, float &d2) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Vector3& point) const;
/// Computes the closest point on this line segment to the given object.
/** @param d [out] If specified, this parameter receives the normalized distance along
this line segment which specifies the closest point on this line segment to
the specified point.
@return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Vector3& point, float& d) const;
/** @param d2 [out] If specified, this parameter receives the (normalized, in case of line segment)
distance along the other line object which specifies the closest point on that line to
this line segment. */
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Ray& other) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Ray& other, float &d) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Ray& other, float &d, float &d2) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Line& other) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Line& other, float &d) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const Line &other, float &d, float &d2) const;
bool Intersects(const LineSegment &segment) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const LineSegment& other) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const LineSegment& other, float &d) const;
/// Computes the closest point on this line segment to the given object.
/** @return The closest point on this line segment to the given object.
@see Contains(), Distance(), Intersects(). */
Vector3 ClosestPoint(const LineSegment& other, float &d, float &d2) const;
[[nodiscard]] float Distance(const Vector3 &point) const;
[[nodiscard]] float DistanceSq(const Vector3& point) const;
[[nodiscard]] float DistanceSq(const LineSegment &other) const;
float Distance(const Vector3 &point, float &d) const;
[[nodiscard]] float Distance(const Ray &other) const;
float Distance(const Ray &other, float &d) const;
float Distance(const Ray &other, float &d, float &d2) const;
[[nodiscard]] float Distance(const Line &other) const;
float Distance(const Line &other, float &d) const;
float Distance(const Line &other, float &d, float &d2) const;
[[nodiscard]] float Distance(const LineSegment &other) const;
float Distance(const LineSegment &other, float &d) const;
float Distance(const LineSegment &other, float &d, float &d2) const;
[[nodiscard]] float Distance(const Plane& other) const;
/// Returns the normalized direction vector that points in the direction a->b.
/** @note The returned vector is normalized, meaning that its length is 1, not |b-a|.
@see a, b. */
[[nodiscard]] Vector3 Dir() const;
/// Computes the length of this line segment.
/** @return |b-a|.
@see a, b. */
[[nodiscard]] float Length() const;
/// Computes the squared length of this line segment.
/** Calling this function is faster than calling Length(), since this function avoids computing a square root.
If you only need to compare lengths to each other and are not interested in the actual length values,
you can compare by using LengthSq(), instead of Length(), since Sqrt() is an order-preserving
(monotonous and non-decreasing) function. [similarOverload: Length] */
[[nodiscard]] float LengthSq() const;
/// Tests if this line segment is finite.
/** A line segment is <b><i>finite</i></b> if its endpoints a and b do not contain floating-point NaNs or +/-infs
in them.
@return True if both a and b have finite floating-point values. */
[[nodiscard]] bool IsFinite() const;
/// Tests whether this line segment and the given object intersect.
/** Both objects are treated as "solid", meaning that if one of the objects is fully contained inside
another, this function still returns true. (for example, if this line segment is contained inside a sphere)
@todo Output intersection point. */
bool Intersects(const Plane& plane) const;
bool Intersects(const Plane& plane, float* d) const;
bool Intersects(const Triangle& triangle, float* d, Vector3* intersectionPoint) const;
bool Intersects(const Sphere& s, Vector3* intersectionPoint = 0, Vector3* intersectionNormal = 0, float* d = 0) const;
bool Intersects(const AABB& aabb, float& dNear, float& dFar) const;
bool Intersects(const AABB& aabb) const;
bool Intersects(const OBB& obb, float& dNear, float& dFar) const;
bool Intersects(const OBB& obb) const;
bool Intersects(const Capsule& capsule) const;
bool Intersects(const Polygon& polygon) const;
bool Intersects(const Frustum& frustum) const;
bool Intersects(const Polyhedron& polyhedron) const;
/** @param epsilon If testing intersection between two line segments, a distance threshold value is used to account
for floating-point inaccuracies. */
bool Intersects(const LineSegment &segment, float epsilon = 1e-3f) const;
// TODO: Implement Circle class.
// TODO: This signature will be moved to bool Intersects(const Disc& disc) const;
//bool IntersectsDisc(const Circle& disc) const;
/// Converts this LineSegment to a Ray.
/** The pos member of the returned Ray will be equal to a, and the dir member equal to Dir().
@see class Ray, ToLine() */
[[nodiscard]] Ray ToRay() const;
/// Converts this LineSegment to a Line.
/** The pos member of the returned Line will be equal to a, and the dir member equal to Dir().
@see class Line, ToRay() */
[[nodiscard]] Line ToLine() const;
/// Tests if this line segment represents the same set of points than the given line segment.
/** @param distanceThreshold Specifies how much distance threshold to allow in the comparison.
@return True if a == rhs.a && b == rhs.b, or, a == rhs.b && b = rhs.a, within the given epsilon. */
bool Equals(const LineSegment &rhs, float distanceThreshold = 1e-3f) const;
/// Compares whether this LineSegment and the given LineSegment are identical bit-by-bit in the underlying representation.
/** @note Prefer using this over e.g. memcmp, since there can be SSE-related padding in the structures. */
//bool BitEquals(const LineSegment &other) const { return a.BitEquals(other.a) && b.BitEquals(other.b); }
/// Ret
};
LineSegment operator *(const Matrix4x4 &transform, const LineSegment &l);

View File

@@ -3,7 +3,7 @@
#include <J3ML/Geometry/Common.h>
#include <vector>
#include "Shape.h"
#include "J3ML/LinearAlgebra.h"
#include <J3ML/LinearAlgebra/Common.h>
namespace J3ML::Geometry {
@@ -13,7 +13,7 @@ namespace J3ML::Geometry {
std::vector<Vector3> vertices;
/// Quickly returns an arbitrary point inside this AABB. Used in GJK intersection test.
Vector3 AnyPointFast() const { return !vertices.empty() ? vertices[0] : Vector3::NaN; }
Vector3 AnyPointFast() const;
AABB MinimalEnclosingAABB() const;
int NumVertices() const;

View File

@@ -1,9 +1,33 @@
#pragma once
#include <vector>
#include <J3ML/LinearAlgebra/Common.h>
#include <J3ML/Geometry/Common.h>
#include <J3ML/J3ML.h>
using namespace J3ML::LinearAlgebra;
namespace J3ML::Geometry
{
class TriangleMesh
{
public:
/// Default constructor for a triangle mesh.
TriangleMesh(int expectedPolygonCount = 1000);
public:
//std::vector<Vector3> Vertices;
//std::vector<Vector3> Normals;
//std::vector<Vector3> UVs;
//std::vector<u64> Indices;
std::vector<float> GenerateVertexList();
//std::vector<Triangle> GenerateTriangleList();
private:
std::vector<float> cachedVertexList;
//std::vector<Triangle> cachedTriangleList;
};
}

View File

@@ -1,14 +1,23 @@
/// Josh's 3D Math Library
/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
/// Developed and Maintained by Josh O'Leary @ Redacted Software.
/// Special Thanks to William Tomasine II and Maxine Hayes.
/// (c) 2024 Redacted Software
/// This work is dedicated to the public domain.
/// @file J3ML.h
/// @desc Core mathematical and utility functions, concrete types, and math constants.
/// @edit 2024-07-05
#pragma once
//
// Created by josh on 12/25/2023.
//
#include <cstdint>
#include <cmath>
#include <string>
#include <cassert>
#include <vector>
// TODO: Move to separate math lib, find duplicates!
/// Swaps two elements in-place without copying their data.
template <typename T>
void Swap(T &a, T &b)
{
@@ -17,6 +26,7 @@ void Swap(T &a, T &b)
b = std::move(temp);
}
/// Clean symbolic names for integers of specific size.
namespace J3ML::SizedIntegralTypes
{
using u8 = uint8_t;
@@ -30,8 +40,6 @@ namespace J3ML::SizedIntegralTypes
using s64 = int64_t;
}
namespace J3ML::SizedFloatTypes
{
using f32 = float;
@@ -43,144 +51,114 @@ namespace J3ML::SizedFloatTypes
using namespace J3ML::SizedIntegralTypes;
using namespace J3ML::SizedFloatTypes;
//On windows there is no shorthand for pi???? - Redacted.
#ifdef _WIN32
#define M_PI 3.14159265358979323846
#endif
namespace J3ML::Math
namespace J3ML::Math::BitTwiddling
{
/// Parses a string of form "011101010" to a u32
u32 BinaryStringToValue(const char* s);
/// Returns the number of 1's set in the given value.
inline int CountBitsSet(u32 value);
}
// TODO: Implement "Wrappers" for most standard math functions.
// We want to later-on implement lookup tables and SSE as conditional macros.
namespace J3ML::Math::Constants {
/// sqrt(2pi) ^ -1
constexpr float RecipSqrt2Pi = 0.3989422804014326779399460599343818684758586311649346576659258296706579258993018385012523339073069364;
/// pi - https://www.mathsisfun.com/numbers/pi.html
constexpr float Pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679;
/// e - https://www.mathsisfun.com/numbers/e-eulers-number.html
constexpr float EulersNumber = 2.7182818284590452353602874713526624977572470936999595749669676277240766303535475945713821785251664274;
/// 2pi - The ratio of a circle's circumferecne to its radius, and the number of radians in one turn.
constexpr float Tau = 6.28318530717958647692;
/// sqrt(2)
constexpr float PythagorasConstant = 1.41421356237309504880;
/// sqrt(3)
constexpr float TheodorusConstant = 1.73205080756887729352;
/// Golden Ratio
constexpr float Phi = 1.61803398874989484820;
/// ln 2
constexpr float NaturalLog2 = 0.6931471805599453094172321214581765680755001343602552541206800094933936219696947156058633269964186875;
/// ln 10
constexpr float NaturalLog10 = 2.3025850929940456840179914546843642076011014886287729760333279009675726096773524802359972050895982983;
constexpr float Infinity = INFINITY;
constexpr float NegativeInfinity = INFINITY;
}
/// This set of functions may be set to use lookup tables or SIMD operations.
/// If no options are set, they will default to using standard library implementation.
#undef USE_LOOKUP_TABLES /// Pre-computed lookup tables.
#undef USE_SSE /// Streaming SIMD Extensions (x86)
#undef USE_NEON /// ARM Vector Processing
#undef USE_AVX /// Advanced Vector Extensions (x86)
namespace J3ML::Math::Functions {
float Radians(float deg); /// Converts the given amount of degrees into radians.
float Degrees(float rad); /// Converts the given amount of radians into degrees.
float Sin(float x); /// Computes the sine of x, in radians.
float Cos(float x); /// Computes the cosine of x, in radians.
float Tan(float x); /// Computes the tangent of x, in radians.
/// Simultaneously computes both sine and cosine of x, in radians.
/// This yields a small performance increase over computing them separately.
/// @see Sin(), Cos().
void SinCos(float x, float& outSin, float& outCos);
float Asin(float x); /// Computes the inverse sine of x, in radians.
float Acos(float x); /// Computes the inverse cosine of x, in radians.
float Atan(float x); /// Computes the inverse tangent of x, in radians.
float Atan2(float y, float x); /// Computes the signed (principal value) inverse tangent of y/x, in radians.
float Sinh(float x); /// Computes the hyperbolic sine of x, in radians.
float Cosh(float x); /// Computes the hyperbolic cosine of x, in radians.
float Tanh(float x); /// Computes the hyperbolic tangent of x, in radians.
bool IsPow2(u32 number); /// Returns true if the given number is a power of 2.
bool IsPow2(u64 number); /// Returns true if the given number is a power of 2.
float PowInt(float base, int exponent); /// Raises the given base to an integral exponent.
float Pow(float base, float exponent); /// Raises the given base to a float exponent.
float Exp(float exp); /// Returns e (the constant 2.71828...) raised to the given power.
float Log(float base, float value); /// Computes a logarithm of the given value in the specified base.
float Log2(float value); /// Computes a logarithm in base-2.
float Ln(float value); /// Computes a logarithm in the natural base (using e as the base).
float Log10(float value); /// Computes a logarithm in base-10;
float Ceil(float f);
int CeilInt(float f);
float Floor(float f);
int FloorInt(float f);
float Round(float f);
int RoundInt(float f);
float Round(float f, float decimalPlaces);
float Sign(float f);
/// Formats larger numbers into shortened 'Truncated' string representations.
/// 2241 -> 2.2k, 55421 -> 55.4k, 1000000 -> 1.0M
std::string Truncate(float input);
/// Clamps the given input value to the range [min, max].
/** @see Clamp01(), Min(), Max(). */
template<typename T>
T Clamp(const T &val, const T &floor, const T &ceil)
{
assert(floor <= ceil);
return val <= ceil ? (val >= floor ? val : floor) : ceil;
}
/// Clamps the given input value to the range [0, 1].
/** @see Clamp(), Min(), Max(). */
template<typename T>
T Clamp01(const T &val) { return Clamp(val, T(0), T(1)); }
bool EqualAbs(float a, float b, float epsilon = 1e-3f);
float RecipFast(float x);
// Coming soon: Units Namespace
// For Dimensional Analysis
/*
namespace Units
{
struct Unit {};
struct Meters : public Unit { };
struct ImperialInches : public Unit {};
struct Time : public Unit {};
struct Grams : public Unit {};
struct MetersPerSecond : public Unit {};
template <typename TUnit>
struct Quantity
{
public:
float Value;
};
struct Mass : public Quantity<Grams> {};
struct Length : public Quantity<Meters> { };
struct Velocity : public Quantity<MetersPerSecond>{ };
class MetrixPrefix
{
public:
std::string Prefix;
std::string Symbol;
int Power;
float InverseMultiply(float input) const
{
return std::pow(input, -Power);
}
float Multiply(float input) const
{
return std::pow(input, Power);
}
};
namespace Prefixes
{
static constexpr MetrixPrefix Tera {"tera", "T", 12};
static constexpr MetrixPrefix Giga {"giga", "G", 9};
static constexpr MetrixPrefix Mega {"mega", "M", 6};
static constexpr MetrixPrefix Kilo {"kilo", "k", 3};
static constexpr MetrixPrefix Hecto {"hecto", "h", 2};
static constexpr MetrixPrefix Deca {"deca", "da", 1};
static constexpr MetrixPrefix None {"", "", 0};
static constexpr MetrixPrefix Deci {"", "", 0};
static constexpr MetrixPrefix Centi {"", "", 0};
static constexpr MetrixPrefix Milli {"", "", 0};
static constexpr MetrixPrefix Micro {"", "", 0};
static constexpr MetrixPrefix Nano {"", "", 0};
static constexpr MetrixPrefix Pico {"", "", 0};
}
Length operator ""_meters(long double value)
{
return {(float)value};
}
Length operator ""_m(long double value)
{
return {(float)value};
}
constexpr Length operator ""_kilometers(long double value)
{
return Length {(float)value};
}
Length operator ""_km(long double value)
{
return {(float)value};
}
Length operator ""_centimeters(long double value)
{
return {(float)value};
}
Length operator ""_cm(long double value)
{
return {(float)value};
}
Length operator ""_millimeters(long double value)
{
return {(float)value};
}
Length operator ""_mm(long double value)
{
return {(float)value};
}
Velocity operator ""_mps(long double value)
{
return {(float)value};
}
Velocity operator ""_meters_per_second(long double value)
{
return {(float)value};
}
Velocity operator ""_kmps(long double value)
{
return {(float)value};
}
}*/
#pragma region Constants
static const float Pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679f;
static const float RecipSqrt2Pi = 0.3989422804014326779399460599343818684758586311649346576659258296706579258993018385012523339073069364f;
static const float GoldenRatio = 1.6180339887498948482045868343656381177203091798057628621354486227052604628189024497072072041893911375f;
#pragma endregion
#pragma region Math Functions
float Radians(float degrees);
float Degrees(float radians);
struct NumberRange
{
float LowerBound;
@@ -192,44 +170,45 @@ namespace J3ML::Math
float NormalizeToRange(float input, const NumberRange& from, const NumberRange& to);
// auto rotation_normalized = NormalizeToRange(inp, {0, 360}, {-1, 1});
inline float Lerp(float a, float b, float t);
float Lerp(float a, float b, float t);
/// Linearly interpolates from a to b, under the modulus mod.
/// This function takes evaluates a and b in the range [0, mod] and takes the shorter path to reach from a to b.
inline float LerpMod(float a, float b, float mod, float t);
float LerpMod(float a, float b, float mod, float t);
/// Computes the lerp factor a and b have to be Lerp()ed to get x.
inline float InverseLerp(float a, float b, float x);
float InverseLerp(float a, float b, float x);
/// See http://msdn.microsoft.com/en-us/library/bb509665(v=VS.85).aspx
inline float Step(float y, float x);
float Step(float y, float x);
/// See http://msdn.microsoft.com/en-us/library/bb509658(v=vs.85).aspx
inline float Ramp(float min, float max, float x);
float Ramp(float min, float max, float x);
inline float PingPongMod(float x, float mod);
float PingPongMod(float x, float mod);
float Sqrt(float x);
float FastSqrt(float x);
inline float Sqrt(float x);
inline float FastSqrt(float x);
/// Returns 1/Sqrt(x). (The reciprocal of the square root of x)
inline float RSqrt(float x);
float RSqrt(float x);
inline float FastRSqrt(float x);
float FastRSqrt(float x);
#pragma endregion
namespace BitTwiddling
{
/// Parses a string of form "011101010" to a u32
u32 BinaryStringToValue(const char* s);
/// Returns the number of 1's set in the given value.
inline int CountBitsSet(u32 value);
}
namespace Interp
{
inline float SmoothStart(float t);
}
}
namespace J3ML::Math {
using namespace Math::Constants;
using namespace Math::Functions;
struct Rotation
{
public:
@@ -249,5 +228,6 @@ namespace J3ML::Math
Rotation operator ""_deg(long double rads);
Rotation operator ""_degrees(long double rads);
}
}

View File

@@ -25,4 +25,7 @@ namespace J3ML::LinearAlgebra
namespace J3ML::LinearAlgebra
{
}
}
using namespace J3ML::LinearAlgebra;

View File

@@ -457,8 +457,6 @@ namespace J3ML::LinearAlgebra {
This occurs if this matrix has a negative determinant.*/
[[nodiscard]] bool HasNegativeScale() const;
/// Returns true if the column and row vectors of this matrix form an orthonormal set.
/// @note In math terms, there does not exist such a thing as an 'orthonormal matrix'. In math terms, a matrix
/// is orthogonal if the column and row vectors are orthogonal *unit* vectors.

View File

@@ -10,8 +10,6 @@ using namespace J3ML::Algorithm;
namespace J3ML::LinearAlgebra {
/// @brief A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
/// This matrix can represent the most generic form of transformations for 3D objects,
/// including perspective projections, which a 4-by-3 cannot store, and translations, which a 3-by-3 cannot represent.

View File

@@ -6,128 +6,265 @@
#include <J3ML/LinearAlgebra/Common.h>
namespace J3ML::LinearAlgebra {
/// A 3D vector of form (x,y,z,w) in a 4D homogeneous coordinate space.
/** This class has two sets of member functions. The functions ending in a suffix '3' operate only on the
(x, y, z) part, ignoring the w component (or assuming a value of 0 or 1, where expectable). The functions
without the '3' suffix operate on all four elements of the vector. */
class Vector4 {
public:
// Default Constructor
enum { Size = 4 };
public:
static const Vector4 Zero;
static const Vector4 NaN;
public:
/// The default constructor does not initialize any members of this class.
/** This means that the values of the members x, y, z, and w are all undefined after creating a new Vector4 using
this default constructor. Remember to assign to them before use.
@see x, y, z, w. */
Vector4();
// Constructs a new Vector4 with x,y,z values from a Vector3
Vector4(const Vector3& xyz, float w = 0);
// Constructs a new Vector4 with the value (X, Y, Z, W)
/// Constructs a new Vector4 with x,y,z values from a Vector3
explicit Vector4(const Vector3& xyz, float w = 0);
/// Constructs a new Vector4 with the value (X, Y, Z, W)
/** @note If you are constructing a float4 from an array of consecutive values, always prefer calling "float4(ptr);" instead of "float4(ptr[0], ptr[1], ptr[2], ptr[3]);"
because there is a considerable SIMD performance benefit in the first form.
@see x, y, z, w. */
Vector4(float X, float Y, float Z, float W);
Vector4(const Vector4& copy) = default;
/// The Vector4 copy constructor.
Vector4(const Vector4& copy) { Set(copy); }
Vector4(Vector4&& move) = default;
Vector4& operator=(const Vector4& rhs);
float* ptr()
{
return &x;
}
Vector3 XYZ() const;
/// Constructs this Vector4 from a C array, to the value (data[0], data[1], data[2], data[3]).
/** @param data An array containing four elements for x, y, z, and w. This pointer may not be null. */
explicit Vector4(const float* data);
float GetX() const { return x; }
float GetY() const { return y; }
float GetZ() const { return z; }
float GetW() const { return w; }
#if MUTABLE
/// Casts this Vector4 to a C array.
/** This function does not allocate new memory or make a copy of this Vector4. This function simply
returns a C pointer view to this data structure. Use ptr()[0] to access the x component of this Vector4,
ptr()[1] to access y, ptr()[2] to access z, and ptr()[3] to access the w component of this Vector4.
@note Since the returned pointer points to this class, do not dereference the pointer after this
Vector has been deleted. You should never store a copy of the returned pointer.
@note This function is provided for compatibility with other APIs which require raw C pointer access
to vectors. Avoid using this function in general, and instead always use the operator [] of this
class to access the elements of this vector by index. */
inline float* ptr();
[[nodiscard]] const float* ptr() const;
/// Accesses an element of this vector using array notation.
/** @param index The element to get. Pass in 0 for x, 1 for y, 2 for z and 3 for w.
@note If you have a non-const instance of this class, you can use this notation to set the elements of
this vector as well, e.g. vec[1] = 10.f; would set the y-component of this vector. */
float operator[](std::size_t index) const;
float &operator[](std::size_t index);
/// Accesses an element of this vector.
/** @param index The element to get. Pass in 0 for x, 1 for y, 2 for z and 3 for w.
@note If you have a non-const instance of this class, you can use this notation to set the elements of
this vector as well, e.g. vec.At(1) = 10.f; would set the y-component of this vector. */
float At(int index) const;
float& At(int index);
/// Returns the (x,y) part of this vector.
[[nodiscard]] Vector2 XY() const;
/// Returns the (x,y,z) part of this vector.
[[nodiscard]] Vector3 XYZ() const;
[[nodiscard]] float GetX() const { return x; }
[[nodiscard]] float GetY() const { return y; }
[[nodiscard]] float GetZ() const { return z; }
[[nodiscard]] float GetW() const { return w; }
void SetX(float newX) { x = newX;}
void SetY(float newY) { y = newY;}
void SetZ(float newZ) { z = newZ;}
void SetW(float newW) { w = newW;}
#endif
static const Vector4 Zero;
static const Vector4 NaN;
float operator[](std::size_t index) const;
float &operator[](std::size_t index);
/// Sets all elements of this vector.
/** @see x, y, z, w, At(). */
void Set(float x, float y, float z, float w);
void Set(const Vector4& rhs);
bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
float LengthSqXYZ() const;
bool IsNormalized3(float epsilonSq = 1e-5f) const
{
return std::abs(LengthSqXYZ()-1.f) <= epsilonSq;
}
bool IsNormalized4(float epsilonSq = 1e-5f) const
{
return std::abs(LengthSquared()-1.f) <= epsilonSq;
}
bool IsNormalized(float epsilonSq = 1e-5f) const { return IsNormalized4(epsilonSq); }
bool IsZero(float epsilonSq = 1e-6f) const;
bool IsFinite() const;
bool IsPerpendicular(const Vector4& other, float epsilonSq=1e-5f) const
{
float dot = Dot(other);
return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
}
bool IsPerpendicular3(const Vector4& other, float epsilonSq = 1e-5f) const
{
}
[[nodiscard]] bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
/// Computes the squared length of the (x,y,z) part of this vector.
[[nodiscard]] float LengthSqXYZ() const;
/// Tests if the length of the (x, y, z) part of this vector is one, up to the given epsilon.
/** @see NormalizeW(), IsWZeroOrOne(), IsZero3(), IsZero4(), IsNormalized4(). */
[[nodiscard]] bool IsNormalized3(float epsilonSq = 1e-5f) const;
/// Returns true if the length of this vector is 1, up to the given epsilon.
/** This function takes into account all the four components of this vector when calculating the norm.
@see NormalizeW(), IsWZeroOrOne(), IsZero3(), IsZero4(), IsNormalized3(). */
[[nodiscard]] bool IsNormalized4(float epsilonSq = 1e-5f) const;
[[nodiscard]] bool IsNormalized(float epsilonSq = 1e-5f) const;
/// Tests if the (x, y, z) part of this vector is equal to (0,0,0), up to the given epsilon.
/** @see NormalizeW(), IsWZeroOrOne(), IsZero4(), IsNormalized3(), IsNormalized4(). */
[[nodiscard]] bool IsZero3(float epsilonSq = 1e-6f) const;
/// Returns true if this vector is equal to (0,0,0,0), up to the given epsilon.
/** @see NormalizeW(), IsWZeroOrOne(), IsZero3(), IsNormalized3(), IsNormalized4(). */
[[nodiscard]] bool IsZero(float epsilonSq = 1e-6f) const;
[[nodiscard]] bool IsZero4(float epsilonSq = 1e-6f) const;
/// Tests if this vector contains valid finite elements.
[[nodiscard]] bool IsFinite() const;
/// Tests if the (x, y, z) parts of two vectors are perpendicular to each other.
[[nodiscard]] bool IsPerpendicular(const Vector4& other, float epsilonSq=1e-5f) const;
[[nodiscard]] bool IsPerpendicular3(const Vector4& other, float epsilonSq = 1e-5f) const;
/// Divides each element by w to produce a Vector4 of form (x, y, z, 1).
/** This function performs the <b>perspective divide</b> or the <b>homogeneous divide</b> on this vector, which is the
process of dividing each element of this vector by w. If the w component of this vector is zero before division, the
result of this vector will be undefined.
@note This function operates in-place.
@see IsWZeroOrOne(). */
void NormalizeW();
bool operator==(const Vector4& rhs) const;
bool operator!=(const Vector4& rhs) const;
bool Equals(const Vector4& rhs, float epsilon = 1e-3f) const;
bool Equals(float _x, float _y, float _z, float _w, float epsilon = 1e-3f) const;
[[nodiscard]] bool Equals(const Vector4& rhs, float epsilon = 1e-3f) const;
[[nodiscard]] bool Equals(float _x, float _y, float _z, float _w, float epsilon = 1e-3f) const;
Vector4 Min(const Vector4& min) const;
Vector4 Max(const Vector4& max) const;
Vector4 Clamp(const Vector4& min, const Vector4& max) const;
float Distance(const Vector4& to) const;
float Length() const;
float LengthSquared() const;
float Magnitude() const;
float Dot(const Vector4& rhs) const;
Vector4 Project(const Vector4& rhs) const;
/// Returns an element-wise minimum of this and the vector (ceil, ceil, ceil, ceil).
/** Each element that is larger than ceil is replaced by ceil. */
[[nodiscard]] Vector4 Min(float ceil) const;
/// Returns an element-wise minimum of this and the given vector.
/** Each element that is larger than ceil is replaced by ceil.
@see Max(), Clamp(). */
[[nodiscard]] Vector4 Min(const Vector4& ceil) const;
/// Returns an element-wise maximum of this and the vector (floor, floor, floor, floor).
/** Each element that is smaller than floor is replaced by floor. */
[[nodiscard]] Vector4 Max(float floor) const;
/// Returns an element-wise maximum of this and the given vector.
/** Each element that is smaller than floor is replaced by floor.
@see Min(), Clamp(). */
[[nodiscard]] Vector4 Max(const Vector4& floor) const;
/// Returns a vector that has floor <= this[i] <= ceil for each element.
[[nodiscard]] Vector4 Clamp(float floor, float ceil) const;
/// Limits each element of this vector between the corresponding elements in floor and ceil.
/** @see Min(), Max(), Clamp01(). */
[[nodiscard]] Vector4 Clamp(const Vector4& floor, const Vector4& ceil) const;
/// Limits each element of this vector in the range [0, 1].
/** @see Min(), Max(), Clamp(). */
[[nodiscard]] Vector4 Clamp01() const;
[[nodiscard]] float Distance(const Vector4& to) const;
/// Computes the length of this vector.
/** @return Sqrt(x*x + y*y + z*z + w*w).
@see LengthSq3(), Length3(), LengthSq4(), Normalize3(), Normalize4(). */
[[nodiscard]] float Length4() const;
[[nodiscard]] float Length() const;
/// Computes the squared length of this vector.
/** Calling this function is faster than calling Length4(), since this function avoids computing a square root.
If you only need to compare lengths to each other, but are not interested in the actual length values,
you can compare by using LengthSq4(), instead of Length4(), since Sqrt() is an order-preserving
(monotonous and non-decreasing) function.
@return x*x + y*y + z*z + w*w.
@see Length3(), LengthSq3(), Length4(), Normalize3(), Normalize4(). */
[[nodiscard]] float LengthSquared4() const;
[[nodiscard]] float LengthSquared() const;
[[nodiscard]] float LengthSq4() const;
[[nodiscard]] float LengthSq() const;
/// Computes the length of the (x, y, z) part of this vector.
/** @note This function ignores the w component of this vector.
@return Sqrt(x*x + y*y + z*z).
@see LengthSq3(), LengthSq4(), Length4(), Normalize3(), Normalize4(). */
[[nodiscard]] float Length3() const { return std::sqrt(x*x + y*y + z*z); }
/// Computes the squared length of the (x, y, z) part of this vector.
/** Calling this function is faster than calling Length3(), since this function avoids computing a square root.
If you only need to compare lengths to each other, but are not interested in the actual length values,
you can compare by using LengthSq3(), instead of Length3(), since Sqrt() is an order-preserving
(monotonous and non-decreasing) function.
@note This function ignores the w component of this vector.
@return x*x + y*y + z*z.
@see Length3(), LengthSq4(), Length4(), Normalize3(), Normalize4(). */
[[nodiscard]] float LengthSq3() const;
[[nodiscard]] float LengthSquared3() const;
[[nodiscard]] float Magnitude() const;
[[nodiscard]] float Dot(const Vector4& rhs) const;
/// Computes the dot product of the (x, y, z) parts of this and the given float4.
/** @note This function ignores the w component of this vector (assumes w=0).
@see Dot4(), Cross3(). */
[[nodiscard]] float Dot3(const Vector3& rhs) const;
[[nodiscard]] float Dot3(const Vector4& rhs) const;
[[nodiscard]] Vector4 Project(const Vector4& rhs) const;
// While it is feasable to compute a cross-product in four dimensions
// the cross product only has the orthogonality property in 3 and 7 dimensions
// You should consider instead looking at Gram-Schmidt Orthogonalization
// to find orthonormal vectors.
Vector4 Cross3(const Vector3& rhs) const;
Vector4 Cross3(const Vector4& rhs) const;
Vector4 Cross(const Vector4& rhs) const { return Cross3(rhs); }
Vector4 Normalize() const;
Vector4 Lerp(const Vector4& goal, float alpha) const;
[[nodiscard]] Vector4 Cross3(const Vector3& rhs) const;
[[nodiscard]] Vector4 Cross3(const Vector4& rhs) const;
[[nodiscard]] Vector4 Cross(const Vector4& rhs) const;
float AngleBetween(const Vector4& rhs) const;
[[nodiscard]] Vector4 Normalize() const;
[[nodiscard]] Vector4 Lerp(const Vector4& goal, float alpha) const;
// Adds two vectors
Vector4 operator+(const Vector4& rhs) const;
Vector4 Add(const Vector4& rhs) const;
[[nodiscard]] float AngleBetween(const Vector4& rhs) const;
/// Adds two vectors. [indexTitle: operators +,-,*,/]
/** This function is identical to the member function Add().
@return float4(x + v.x, y + v.y, z + v.z, w + v.w); */
Vector4 operator +(const Vector4& rhs) const;
/// Adds a vector to this vector. [IndexTitle: Add/Sub/Mul/Div]
/// @return (x+v.x, y+v.y, z+v.z, w+v.w).
[[nodiscard]] Vector4 Add(const Vector4& rhs) const;
static Vector4 Add(const Vector4& lhs, const Vector4& rhs);
// Subtracts two vectors
Vector4 operator-(const Vector4& rhs) const;
Vector4 Sub(const Vector4& rhs) const;
/// Subtracts the given vector from this vector. [similarOverload: operator+] [hideIndex]
/** This function is identical to the member function Sub().
@return float4(x - v.x, y - v.y, z - v.z, w - v.w); */
Vector4 operator -(const Vector4& rhs) const;
[[nodiscard]] Vector4 Sub(const Vector4& rhs) const;
static Vector4 Sub(const Vector4& lhs, const Vector4& rhs);
// Multiplies this vector by a scalar value
Vector4 operator*(float rhs) const;
Vector4 Mul(float scalar) const;
/// Multiplies this vector by a scalar. [similarOverload: operator+] [hideIndex]
/** This function is identical to the member function Mul().
@return float4(x * scalar, y * scalar, z * scalar, w * scalar); */
Vector4 operator *(float rhs) const;
[[nodiscard]] Vector4 Mul(float scalar) const;
static Vector4 Mul(const Vector4& lhs, float rhs);
// Divides this vector by a scalar
Vector4 operator/(float rhs) const;
Vector4 Div(float scalar) const;
/// Divides this vector by a scalar. [similarOverload: operator+] [hideIndex]
/** This function is identical to the member function Div().
@return float4(x / scalar, y / scalar, z / scalar, w * scalar); */
Vector4 operator /(float rhs) const;
[[nodiscard]] Vector4 Div(float scalar) const;
static Vector4 Div(const Vector4& rhs, float scalar);
Vector4 operator+() const; // Unary + Operator
Vector4 operator-() const; // Unary - Operator (Negation)
Vector4 operator +() const; // Unary + Operator
/// Performs an unary negation of this vector. [similarOverload: operator+] [hideIndex]
/** This function is identical to the member function Neg().
@return float4(-x, -y, -z, -w). */
Vector4 operator -() const;
public:
#if MUTABLE
float x;
float y;
float z;
float w;
#else
float x = 0;
float y = 0;
float z = 0;
float w = 0;
#endif
};
}

7
include/J3ML/SSE.hpp Normal file
View File

@@ -0,0 +1,7 @@
#pragma once
namespace J3ML
{
}

View File

@@ -4,6 +4,11 @@
int main(int argc, char** argv)
{
for (int i = 10; i < 9999999; i*=1.5f) {
std::cout << J3ML::Math::Functions::Truncate(i) << std::endl;
}
std::cout << "j3ml demo coming soon" << std::endl;
return 0;
}

View File

@@ -274,6 +274,24 @@ namespace J3ML::Geometry {
Vector3 d = obb.r.x * absAxis0 + obb.r.y * absAxis1 + obb.r.z * absAxis2;
}
void AABB::Enclose(const Sphere &sphere) {
Vector3 radiusCubed(sphere.Radius);
Enclose(sphere.Position - radiusCubed, sphere.Position + radiusCubed);
}
void AABB::Enclose(const Triangle &triangle) {
// TODO: Implement alternate min or comparison operators for Vector3
Enclose(Vector3::Min(triangle.V0, triangle.V1, triangle.V2), Vector3::Max(triangle.V0, triangle.V1, triangle.V2));
}
void AABB::Enclose(const Capsule &capsule) {
Vector3 radiusCubed(capsule.r);
minPoint = Vector3::Min(minPoint, Vector3::Min(capsule.l.A, capsule.l.B) - radiusCubed);
maxPoint = Vector3::Max(maxPoint, Vector3::Max(capsule.l.A, capsule.l.B) - radiusCubed);
Enclose(minPoint, maxPoint);
}
Vector3 AABB::GetClosestPoint(const Vector3 &point) const {
Vector3 result = point;
if (point.x > this->maxPoint.x)
@@ -460,6 +478,49 @@ namespace J3ML::Geometry {
return Intersection(aabb).has_value();
}
TriangleMesh AABB::Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const {
TriangleMesh mesh;
assert(numFacesX >= 1);
assert(numFacesX >= 1);
assert(numFacesX >= 1);
// Generate both X-Y planes.
int i = 0;
for (int face = 0; face < 6; ++face) // Faces run in the order -X, +X, -Y, +Y, -Z, +Z.
{
int numFacesU;
int numFacesV;
bool flip = (face == 1 || face == 2 || face == 5);
if (ccwIsFrontFacing)
flip = !flip;
if (face == 0 || face == 1) {
numFacesU = numFacesY;
numFacesV = numFacesZ;
} else if (face == 2 || face == 3) {
numFacesU = numFacesX;
numFacesV = numFacesZ;
} else // if (face == 4 || face == 5)
{
numFacesU = numFacesX;
numFacesV = numFacesY;
}
for (int x = 0; x < numFacesU; ++x) {
for (int y = 0; numFacesV; ++y) {
float u = (float)x / (numFacesU);
float v = (float)y / (numFacesV);
float u2 = (float)(x+1) / (numFacesU);
float v2 = (float)(y+1) / (numFacesV);
//mesh.Vertices[i] = FacePoint(face, u, v);
//mesh.Vertices[i+1] =
// TODO: Come back to this once TriangleMesh is proper fleshed out
}
}
}
}
std::optional<AABB> AABB::Intersection(const AABB& rhs) const {
// Here we do SAT, except that due to both objects being AABBs, they are "already projected" onto the same axis
constexpr auto test = [](float a, float b, float c, float d) -> std::optional<Vector2> {

View File

@@ -106,6 +106,14 @@ namespace J3ML::Geometry
return polyhedron.Intersects(*this);
}
Sphere Capsule::SphereA() const {
return Sphere(l.A, r);
}
Sphere Capsule::SphereB() const {
return Sphere(l.B, r);
}
Vector3 Capsule::ExtremePoint(const Vector3 &direction) const {
float len = direction.Length();
assert(len > 0.f);
@@ -118,6 +126,14 @@ namespace J3ML::Geometry
return extremePoint;
}
bool Capsule::IsDegenerate() const {
return r <= 0.f; // Ask Bill :p
}
float Capsule::Height() const {
return LineLength() + Diameter();
}
Capsule Capsule::Translated(const Vector3 &offset) const
{
return Capsule(l.A + offset, l.B + offset, r);

View File

@@ -1,6 +1,9 @@
#include <J3ML/Geometry/LineSegment.h>
#include "J3ML/Geometry/Capsule.h"
#include <J3ML/Geometry/Line.h>
#include <J3ML/LinearAlgebra/Vector3.h>
#include <J3ML/LinearAlgebra/Matrix3x3.h>
#include <J3ML/Geometry/Plane.h>
namespace J3ML::Geometry {
@@ -9,6 +12,10 @@ namespace J3ML::Geometry {
}
LineSegment::LineSegment(const Ray &ray, float d): A(ray.Origin), B(ray.GetPoint(d)) {}
LineSegment::LineSegment(const Line &line, float d): A(line.Position), B(line.GetPoint(d)) {}
LineSegment::LineSegment() {}
void LineSegment::ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const
@@ -24,6 +31,10 @@ namespace J3ML::Geometry {
return (1.f - d) * A + d * B;
}
Vector3 LineSegment::CenterPoint() const { return (A+B) * 0.5f;}
void LineSegment::Reverse() { Swap(A, B);}
float LineSegment::Distance(const Vector3 &point, float &d) const
{
/// See Christer Ericson's Real-Time Collision Detection, p.130.
@@ -73,10 +84,23 @@ namespace J3ML::Geometry {
Vector3 LineSegment::ClosestPoint(const Vector3 &point, float &d) const {
Vector3 dir = B - A;
d = Clamp01(Vector3::Dot(point - A, dir) / dir.LengthSquared());
d = Math::Clamp01(Vector3::Dot(point - A, dir) / dir.LengthSquared());
return A + d * dir;
}
Vector3 LineSegment::ClosestPoint(const Ray &other) const { float d, d2; return ClosestPoint(other, d, d2);}
Vector3 LineSegment::ClosestPoint(const Ray &other, float &d) const { float d2; return ClosestPoint(other, d, d2);}
Vector3 LineSegment::ClosestPoint(const Ray &other, float &d, float &d2) const {
other.ClosestPoint(*this, d2, d);
return GetPoint(d);
}
Vector3 LineSegment::ClosestPoint(const Line &other) const { float d, d2; return ClosestPoint(other, d, d2); }
Vector3 LineSegment::ClosestPoint(const Line &other, float &d) const { float d2; return ClosestPoint(other, d, d2); }
Vector3 LineSegment::ClosestPoint(const Line &other, float &d, float &d2) const
{
Line::ClosestPointLineLine(other.Position, other.Direction, A,B - A, d2, d);
@@ -96,10 +120,9 @@ namespace J3ML::Geometry {
return GetPoint(d);
}
Vector3 LineSegment::ClosestPoint(const Ray &other, float &d, float &d2) const {
other.ClosestPoint(*this, d2, d);
return GetPoint(d);
}
Vector3 LineSegment::ClosestPoint(const LineSegment &other) const { float d, d2; return ClosestPoint(other, d, d2); }
Vector3 LineSegment::ClosestPoint(const LineSegment &other, float &d) const { float d2; return ClosestPoint(other, d, d2); }
Vector3 LineSegment::ClosestPoint(const LineSegment &other, float &d, float &d2) const
{
@@ -209,10 +232,14 @@ namespace J3ML::Geometry {
return A.DistanceSq(B);
}
bool LineSegment::Intersects(const LineSegment &segment) const {
bool LineSegment::IsFinite() const { return A.IsFinite() && B.IsFinite(); }
bool LineSegment::Intersects(const LineSegment &segment, float epsilon) const {
return false;
}
Vector3 LineSegment::AnyPointFast() const { return A; }
Vector3 LineSegment::ExtremePoint(const Vector3 &direction) const {
return Vector3::Dot(direction, B-A) >= 0.f ? B : A;
}
@@ -234,6 +261,14 @@ namespace J3ML::Geometry {
return GetPoint(d).Distance(other.GetPoint(d2));
}
Ray LineSegment::ToRay() const {
return Ray(A, Dir());
}
Line LineSegment::ToLine() const {
return Line(A, Dir());
}
LineSegment operator*(const Matrix4x4 &transform, const LineSegment &l) {
return LineSegment(transform.Mul(l.A), transform.Mul(l.B));
}

View File

@@ -6,6 +6,8 @@
#include <J3ML/Algorithm/GJK.h>
namespace J3ML::Geometry {
Vector3 Polygon::AnyPointFast() const { return !vertices.empty() ? vertices[0] : Vector3::NaN; }
AABB Polygon::MinimalEnclosingAABB() const {
AABB aabb;
aabb.SetNegativeInfinity();

View File

@@ -1 +1,10 @@
#include <J3ML/Geometry/TriangleMesh.h>
#include <J3ML/Geometry/TriangleMesh.h>
#include <J3ML/LinearAlgebra/Vector3.h>
TriangleMesh::TriangleMesh(int expectedPolygonCount) {
//Vertices.reserve(expectedPolygonCount);
//Normals.reserve(expectedPolygonCount);
//UVs.reserve(expectedPolygonCount);
//Indices.reserve(expectedPolygonCount);
}

View File

@@ -1,34 +1,129 @@
/// Josh's 3D Math Library
/// A C++20 Library for 3D Math, Computer Graphics, and Scientific Computing.
/// Developed and Maintained by Josh O'Leary @ Redacted Software.
/// Special Thanks to William Tomasine II and Maxine Hayes.
/// (c) 2024 Redacted Software
/// This work is dedicated to the public domain.
/// @file J3ML.cpp
/// @edit 2024-07-05
#include <format>
#include <iomanip>
#include <strstream>
#include <J3ML/J3ML.h>
float PowUInt(float base, u32 exponent)
{
// 'Fast Exponentiation': We interpret exponent in base two and calculate the power by
// squaring and multiplying by base.
// Find the highest bit that is set.
u32 e = 0x80000000;
while((exponent & e) == 0 && e > 0)
e >>= 1;
float val = 1.f;
do
{
val *= val; // Shifts the exponent one place left
val *= (exponent & e) != 0 ? base : 1.f; // Adds a 1 as the LSB of the exponent
e >>= 1;
} while(e > 0);
return val;
}
namespace J3ML
{
Math::Rotation Math::operator ""_degrees(long double rads) { return {Radians((float)rads)}; }
float Math::Functions::Radians(float degrees) { return degrees * (Pi/180.f); }
Math::Rotation Math::operator ""_deg(long double rads) { return {Radians((float)rads)}; }
float Math::Functions::Degrees(float radians) { return radians * (180.f/Pi); }
Math::Rotation Math::operator ""_degrees(long double rads) { return {Functions::Radians((float)rads)}; }
Math::Rotation Math::operator ""_deg(long double rads) { return {Functions::Radians((float)rads)}; }
Math::Rotation Math::operator ""_radians(long double rads) { return {(float)rads}; }
Math::Rotation Math::operator ""_rad(long double rads) { return {(float)rads}; }
float Math::FastRSqrt(float x) {
float Math::Functions::FastRSqrt(float x) {
return 1.f / FastSqrt(x);
}
float Math::RSqrt(float x) {
float Math::Functions::Sqrt(float x) {
return std::sqrt(x);
}
float Math::Functions::FastSqrt(float x) {
// TODO: implement FastSqrt from MGL
return std::sqrt(x);
}
float Math::Functions::RSqrt(float x) {
return 1.f / Sqrt(x);
}
float Math::Radians(float degrees) { return degrees * (Pi/180.f); }
float Math::Degrees(float radians) { return radians * (180.f/Pi); }
bool Math::EqualAbs(float a, float b, float epsilon) {
int SigFigsTable[] = {0,0,0,1,0,0,1,0,0,1};
int DivBy[] = {1,1,1, 1000,1000,1000, 1000000, 1000000, 1000000, 1000000000, 1000000000,1000000000};
std::vector<std::string> Suffixes = {
"", "", "",
"K", "K", "K",
"M", "M", "M",
"B", "B", "B",
"T", "T", "T",
"Q", "Q", "Q"
};
float Math::Functions::Round(float f, float decimalPlaces) {
float mult = Pow(10, decimalPlaces);
return Floor(f * mult + 0.5f) / mult;
}
float Math::Functions::Sign(float f) { return f >= 0.f ? 1.f : -1.f;}
std::string Math::Functions::Truncate(float input) {
std::stringstream ss;
std::string str = "";
if (input < 1000)
ss << std::fixed << std::setprecision(0) << input;
else {
int figs = CeilInt(Log10(input)) - 1;
auto suffix = Suffixes[figs];
auto roundTo = SigFigsTable[figs];
auto divBy = DivBy[figs];
auto fractional = input / (float)divBy;
// Increment roundTo for extra precision!!
ss << std::fixed << std::setprecision(roundTo) << fractional << suffix;
}
str = ss.str();
return str;
}
bool Math::Functions::EqualAbs(float a, float b, float epsilon) {
return std::abs(a - b) < epsilon;
}
float Math::RecipFast(float x) {
float Math::Functions::RecipFast(float x) {
// TODO: Implement SSE rcp instruction.
return 1.f / x;
}
@@ -49,4 +144,91 @@ namespace J3ML
int Math::BitTwiddling::CountBitsSet(u32 value) {
}
}
namespace Math::Functions {
float Sin(float x) {
#ifdef USE_LOOKUP_TABLES
#elif USE_SSE
#else
return std::sin(x);
#endif
}
float Cos(float x) {
#ifdef USE_LOOKUP_TABLES
#elif USE_SSE
#else
return std::cos(x);
#endif
}
float Tan(float x) { return std::tanf(x); }
void SinCos(float x, float &outSin, float &outCos) {
outSin = Sin(x);
outCos = Cos(x);
}
float Asin(float x) { return std::asinf(x); }
float Acos(float x) { return std::acosf(x); }
float Atan(float x) { return std::atanf(x); }
float Atan2(float y, float x) { return std::atan2f(y, x); }
float Sinh(float x) { return std::sinhf(x); }
float Cosh(float x) { return std::coshf(x); }
float Tanh(float x) { return std::tanhf(x); }
bool IsPow2(u32 number) {
return (number & (number - 1)) == 0;
}
bool IsPow2(u64 number) {
return (number & (number - 1)) == 0;
}
float PowInt(float base, int exponent) {
if (exponent == 0)
return 1.f;
else if (exponent < 0)
return 1.f / PowUInt(base, (u32) -exponent);
else return PowUInt(base, (u32) exponent);
}
float Pow(float base, float exponent) { return std::powf(base, exponent); }
float Exp(float exp) { return std::exp(exp); }
float Log(float base, float value) {
return std::log(value) / std::log(base);
}
float Log2(float value) {
return Log(2.f, value);
}
float Ln(float value) {
return std::log(value);
}
float Log10(float value) {
return Log(10, value);
}
float Ceil(float f) { return std::ceilf(f); }
int CeilInt(float f) { return (int)std::ceilf(f);}
float Floor(float x) { return floorf(x); }
int FloorInt(float x) { return (int)floorf(x); }
float Round(float x) { return Floor(x+0.5f); }
int RoundInt(float x) { return (int)Round(x); }
}
}

View File

@@ -405,7 +405,7 @@ namespace J3ML::LinearAlgebra {
}
Vector4 Matrix3x3::Mul(const Vector4 &rhs) const {
return {Mul(rhs.XYZ()), rhs.GetW()};
return Vector4(Mul(rhs.XYZ()), rhs.GetW());
}
Vector3 Matrix3x3::Mul(const Vector3 &rhs) const {

View File

@@ -9,6 +9,9 @@
namespace J3ML::LinearAlgebra {
using namespace J3ML::Math;
const Vector4 Vector4::Zero = {0,0,0,0};
const Vector4 Vector4::NaN = {NAN, NAN, NAN, NAN};
@@ -18,6 +21,30 @@ namespace J3ML::LinearAlgebra {
Vector4::Vector4(float X, float Y, float Z, float W): x(X),y(Y),z(Z),w(W)
{ }
bool Vector4::IsFinite() const {
return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && std::isfinite(w);
}
bool Vector4::IsPerpendicular(const Vector4 &other, float epsilonSq) const {
float dot = Dot(other);
return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
}
bool Vector4::IsPerpendicular3(const Vector4 &other, float epsilonSq) const {
float dot = Dot3(other);
return dot*dot <= epsilonSq * LengthSq() * other.LengthSq();
}
void Vector4::NormalizeW() {
if (std::abs(w) > 1e-6f) {
float invW = 1.f / w;
x *= invW;
y *= invW;
z *= invW;
w = 1.f;
}
}
bool Vector4::operator==(const Vector4& rhs) const
{
return this->IsWithinMarginOfError(rhs);
@@ -38,6 +65,10 @@ Vector4 Vector4::Min(const Vector4& min) const
};
}
Vector4 Vector4::Max(float floor) const {
return {std::max(x, floor), std::max(y, floor), std::max(z, floor), std::max(w, floor)};
}
Vector4 Vector4::Max(const Vector4& max) const
{
return {
@@ -48,6 +79,14 @@ Vector4 Vector4::Max(const Vector4& max) const
};
}
Vector4 Vector4::Clamp(float floor, float ceil) const {
return {Math::Clamp(x, floor, ceil),
Math::Clamp(y, floor, ceil),
Math::Clamp(z, floor, ceil),
Math::Clamp(w, floor, ceil)
};
}
Vector4 Vector4::Clamp(const Vector4& min, const Vector4& max) const
{
return {
@@ -63,9 +102,14 @@ float Vector4::Distance(const Vector4& to) const
return ( (*this) - to ).Magnitude();
}
float Vector4::Length() const
{
return std::sqrt(LengthSquared());
float Vector4::Length4() const {
return std::sqrt(x*x + y*y + z*z + w*w);
}
float Vector4::Length() const { return Length4(); }
float Vector4::LengthSquared4() const {
return x*x + y*y + z*z + w*w;
}
float Vector4::LengthSquared() const
@@ -73,6 +117,14 @@ float Vector4::LengthSquared() const
return (x*x + y*y + z*z + w*w);
}
float Vector4::LengthSq4() const { return LengthSquared4();}
float Vector4::LengthSq() const { return LengthSquared();}
float Vector4::LengthSquared3() const { return LengthSq3(); }
float Vector4::LengthSq3() const { return x*x + y*y + z*z; }
float Vector4::Magnitude() const
{
return std::sqrt(x*x + y*y + z*z + w*w);
@@ -89,6 +141,14 @@ float Vector4::Dot(const Vector4& rhs) const
a.w * b.w;
}
float Vector4::Dot3(const Vector3 &rhs) const {
return x * rhs.x + y * rhs.y + z * rhs.z;
}
float Vector4::Dot3(const Vector4 &rhs) const {
return x * rhs.x + y * rhs.y + z * rhs.z;
}
Vector4 Vector4::Project(const Vector4& rhs) const
{
float scalar = this->Dot(rhs) / (rhs.Magnitude()* rhs.Magnitude());
@@ -150,8 +210,34 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
return *this;
}
Vector3 Vector4::XYZ() const {
return {x, y, z};
Vector4::Vector4(const float *data) {
assert(data);
x = data[0];
y = data[1];
z = data[2];
w = data[3];
}
float * Vector4::ptr() { return &x; }
const float * Vector4::ptr() const { return &x; }
Vector2 Vector4::XY() const { return {x,y}; }
Vector3 Vector4::XYZ() const { return {x, y, z}; }
void Vector4::Set(float x_, float y_, float z_, float w_) {
this->x = x_;
this->y = y_;
this->z = z_;
this->w = w_;
}
void Vector4::Set(const Vector4 &rhs) {
x = rhs.x;
y = rhs.y;
z = rhs.z;
w = rhs.w;
}
bool Vector4::Equals(const Vector4 &rhs, float epsilon) const {
@@ -168,6 +254,10 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
std::abs(w - _w) < epsilon;
}
Vector4 Vector4::Min(float ceil) const {
return {std::min(x, ceil), std::min(y, ceil), std::min(z, ceil), std::min(w, ceil)};
}
float Vector4::operator[](std::size_t index) const {
assert(index < 4);
if (index==0) return x;
@@ -186,10 +276,36 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
}
float Vector4::At(int index) const {
assert(index >= 0);
assert(index < Size);
return ptr()[index];
}
float & Vector4::At(int index) {
assert(index >= 0);
assert(index < Size);
return ptr()[index];
}
float Vector4::LengthSqXYZ() const {
return x*x * y*y * z*z;
}
bool Vector4::IsNormalized3(float epsilonSq) const {
return std::abs(LengthSqXYZ()-1.f) <= epsilonSq;
}
bool Vector4::IsNormalized4(float epsilonSq) const {
return std::abs(LengthSquared()-1.f) <= epsilonSq;
}
bool Vector4::IsNormalized(float epsilonSq) const { return IsNormalized4(epsilonSq); }
bool Vector4::IsZero3(float epsilonSq) const {
return LengthSq3() <= epsilonSq;
}
Vector4 Vector4::Cross3(const Vector3 &rhs) const {
Vector4 dst;
dst.x = y * rhs.z - z * rhs.y;
@@ -203,6 +319,6 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
return Cross3(rhs.XYZ());
}
Vector4 Vector4::Cross(const Vector4 &rhs) const { return Cross3(rhs); }
}
#pragma endregion

View File

@@ -4,6 +4,19 @@
using namespace J3ML::LinearAlgebra;
void Matrix4x4_AngleTypeRoundtripConversion()
{
Matrix4x4 matrix;
EulerAngle a(Math::Radians(45), Math::Radians(45), Math::Radians(45));
Quaternion q(a);
matrix.SetRotatePart(q);
//matrix.SetRotatePartX(a.pitch);
//matrix.SetRotatePartY(a.yaw);
//matrix.SetRotatePartZ(a.roll);
EulerAngle fromMatrix = matrix.GetRotatePart().ToQuat().ToEulerAngle();
jtest::check(a == fromMatrix);
}
int Matrix4x4Tests() {
TEST("Mat4x4::Add_Unary", []
@@ -107,6 +120,9 @@ int Matrix4x4Tests() {
TEST("Mat4x4::DeterminantCorrectness", [] { });
TEST("Mat4x4::MulMat3x3", [] {});
TEST("Mat4x4::AngleTypeRoundtripConversion", Matrix4x4_AngleTypeRoundtripConversion);
return 0;
}