Added Kraken Math files, extracted from Kraken Engine
This commit is contained in:
372
src/aabb.cpp
Normal file
372
src/aabb.cpp
Normal file
@@ -0,0 +1,372 @@
|
||||
//
|
||||
// aabb.cpp
|
||||
// Kraken
|
||||
//
|
||||
// Copyright 2018 Kearwood Gilbert. All rights reserved.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without modification, are
|
||||
// permitted provided that the following conditions are met:
|
||||
//
|
||||
// 1. Redistributions of source code must retain the above copyright notice, this list of
|
||||
// conditions and the following disclaimer.
|
||||
//
|
||||
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
|
||||
// of conditions and the following disclaimer in the documentation and/or other materials
|
||||
// provided with the distribution.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY KEARWOOD GILBERT ''AS IS'' AND ANY EXPRESS OR IMPLIED
|
||||
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
|
||||
// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL KEARWOOD GILBERT OR
|
||||
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
// ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
||||
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
|
||||
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//
|
||||
// The views and conclusions contained in the software and documentation are those of the
|
||||
// authors and should not be interpreted as representing official policies, either expressed
|
||||
// or implied, of Kearwood Gilbert.
|
||||
//
|
||||
|
||||
#include "../include/kraken-math.h"
|
||||
#include "assert.h"
|
||||
#include "krhelpers.h"
|
||||
|
||||
namespace kraken {
|
||||
|
||||
void AABB::init()
|
||||
{
|
||||
min = Vector3::Min();
|
||||
max = Vector3::Max();
|
||||
}
|
||||
|
||||
AABB AABB::Create()
|
||||
{
|
||||
AABB r;
|
||||
r.init();
|
||||
return r;
|
||||
}
|
||||
|
||||
void AABB::init(const Vector3 &minPoint, const Vector3 &maxPoint)
|
||||
{
|
||||
min = minPoint;
|
||||
max = maxPoint;
|
||||
}
|
||||
|
||||
AABB AABB::Create(const Vector3 &minPoint, const Vector3 &maxPoint)
|
||||
{
|
||||
AABB r;
|
||||
r.init(minPoint, maxPoint);
|
||||
return r;
|
||||
}
|
||||
|
||||
void AABB::init(const Vector3 &corner1, const Vector3 &corner2, const Matrix4 &modelMatrix)
|
||||
{
|
||||
for(int iCorner=0; iCorner<8; iCorner++) {
|
||||
Vector3 sourceCornerVertex = Matrix4::DotWDiv(modelMatrix, Vector3::Create(
|
||||
(iCorner & 1) == 0 ? corner1.x : corner2.x,
|
||||
(iCorner & 2) == 0 ? corner1.y : corner2.y,
|
||||
(iCorner & 4) == 0 ? corner1.z : corner2.z));
|
||||
|
||||
|
||||
if(iCorner == 0) {
|
||||
min = sourceCornerVertex;
|
||||
max = sourceCornerVertex;
|
||||
} else {
|
||||
if(sourceCornerVertex.x < min.x) min.x = sourceCornerVertex.x;
|
||||
if(sourceCornerVertex.y < min.y) min.y = sourceCornerVertex.y;
|
||||
if(sourceCornerVertex.z < min.z) min.z = sourceCornerVertex.z;
|
||||
if(sourceCornerVertex.x > max.x) max.x = sourceCornerVertex.x;
|
||||
if(sourceCornerVertex.y > max.y) max.y = sourceCornerVertex.y;
|
||||
if(sourceCornerVertex.z > max.z) max.z = sourceCornerVertex.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
AABB AABB::Create(const Vector3 &corner1, const Vector3 &corner2, const Matrix4 &modelMatrix)
|
||||
{
|
||||
AABB r;
|
||||
r.init(corner1, corner2, modelMatrix);
|
||||
return r;
|
||||
}
|
||||
|
||||
bool AABB::operator ==(const AABB& b) const
|
||||
{
|
||||
return min == b.min && max == b.max;
|
||||
}
|
||||
|
||||
bool AABB::operator !=(const AABB& b) const
|
||||
{
|
||||
return min != b.min || max != b.max;
|
||||
}
|
||||
|
||||
Vector3 AABB::center() const
|
||||
{
|
||||
return (min + max) * 0.5f;
|
||||
}
|
||||
|
||||
Vector3 AABB::size() const
|
||||
{
|
||||
return max - min;
|
||||
}
|
||||
|
||||
float AABB::volume() const
|
||||
{
|
||||
Vector3 s = size();
|
||||
return s.x * s.y * s.z;
|
||||
}
|
||||
|
||||
void AABB::scale(const Vector3 &s)
|
||||
{
|
||||
Vector3 prev_center = center();
|
||||
Vector3 prev_size = size();
|
||||
Vector3 new_scale = Vector3::Create(prev_size.x * s.x,
|
||||
prev_size.y * s.y,
|
||||
prev_size.z * s.z) * 0.5f;
|
||||
min = prev_center - new_scale;
|
||||
max = prev_center + new_scale;
|
||||
}
|
||||
|
||||
void AABB::scale(float s)
|
||||
{
|
||||
scale(Vector3::Create(s));
|
||||
}
|
||||
|
||||
bool AABB::operator >(const AABB& b) const
|
||||
{
|
||||
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
|
||||
if(min > b.min) {
|
||||
return true;
|
||||
} else if(min < b.min) {
|
||||
return false;
|
||||
} else if(max > b.max) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
bool AABB::operator <(const AABB& b) const
|
||||
{
|
||||
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
|
||||
|
||||
if(min < b.min) {
|
||||
return true;
|
||||
} else if(min > b.min) {
|
||||
return false;
|
||||
} else if(max < b.max) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool AABB::intersects(const AABB& b) const
|
||||
{
|
||||
// Return true if the two volumes intersect
|
||||
return min.x <= b.max.x && min.y <= b.max.y && min.z <= b.max.z && max.x >= b.min.x && max.y >= b.min.y && max.z >= b.min.z;
|
||||
}
|
||||
|
||||
bool AABB::contains(const AABB &b) const
|
||||
{
|
||||
// Return true if the passed KRAABB is entirely contained within this KRAABB
|
||||
return b.min.x >= min.x && b.min.y >= min.y && b.min.z >= min.z && b.max.x <= max.x && b.max.y <= max.y && b.max.z <= max.z;
|
||||
}
|
||||
|
||||
bool AABB::contains(const Vector3 &v) const
|
||||
{
|
||||
return v.x >= min.x && v.x <= max.x && v.y >= min.y && v.y <= max.y && v.z >= min.z && v.z <= max.z;
|
||||
}
|
||||
|
||||
AABB AABB::Infinite()
|
||||
{
|
||||
return AABB::Create(Vector3::Min(), Vector3::Max());
|
||||
}
|
||||
|
||||
AABB AABB::Zero()
|
||||
{
|
||||
return AABB::Create(Vector3::Zero(), Vector3::Zero());
|
||||
}
|
||||
|
||||
float AABB::longest_radius() const
|
||||
{
|
||||
float radius1 = (center() - min).magnitude();
|
||||
float radius2 = (max - center()).magnitude();
|
||||
return radius1 > radius2 ? radius1 : radius2;
|
||||
}
|
||||
|
||||
|
||||
bool AABB::intersectsLine(const Vector3 &v1, const Vector3 &v2) const
|
||||
{
|
||||
Vector3 dir = Vector3::Normalize(v2 - v1);
|
||||
float length = (v2 - v1).magnitude();
|
||||
|
||||
// EZ cases: if the ray starts inside the box, or ends inside
|
||||
// the box, then it definitely hits the box.
|
||||
// I'm using this code for ray tracing with an octree,
|
||||
// so I needed rays that start and end within an
|
||||
// octree node to COUNT as hits.
|
||||
// You could modify this test to (ray starts inside and ends outside)
|
||||
// to qualify as a hit if you wanted to NOT count totally internal rays
|
||||
if( contains( v1 ) || contains( v2 ) )
|
||||
return true ;
|
||||
|
||||
// the algorithm says, find 3 t's,
|
||||
Vector3 t ;
|
||||
|
||||
// LARGEST t is the only one we need to test if it's on the face.
|
||||
for(int i = 0 ; i < 3 ; i++) {
|
||||
if( dir[i] > 0 ) { // CULL BACK FACE
|
||||
t[i] = ( min[i] - v1[i] ) / dir[i];
|
||||
} else {
|
||||
t[i] = ( max[i] - v1[i] ) / dir[i];
|
||||
}
|
||||
}
|
||||
|
||||
int mi = 0;
|
||||
if(t[1] > t[mi]) mi = 1;
|
||||
if(t[2] > t[mi]) mi = 2;
|
||||
if(t[mi] >= 0 && t[mi] <= length) {
|
||||
Vector3 pt = v1 + dir * t[mi];
|
||||
|
||||
// check it's in the box in other 2 dimensions
|
||||
int o1 = ( mi + 1 ) % 3 ; // i=0: o1=1, o2=2, i=1: o1=2,o2=0 etc.
|
||||
int o2 = ( mi + 2 ) % 3 ;
|
||||
|
||||
return pt[o1] >= min[o1] && pt[o1] <= max[o1] && pt[o2] >= min[o2] && pt[o2] <= max[o2];
|
||||
}
|
||||
|
||||
return false ; // the ray did not hit the box.
|
||||
}
|
||||
|
||||
bool AABB::intersectsRay(const Vector3 &v1, const Vector3 &dir) const
|
||||
{
|
||||
/*
|
||||
Fast Ray-Box Intersection
|
||||
by Andrew Woo
|
||||
from "Graphics Gems", Academic Press, 1990
|
||||
*/
|
||||
|
||||
// FINDME, TODO - Perhaps there is a more efficient algorithm, as we don't actually need the exact coordinate of the intersection
|
||||
|
||||
enum {
|
||||
RIGHT = 0,
|
||||
LEFT = 1,
|
||||
MIDDLE = 2
|
||||
} quadrant[3];
|
||||
|
||||
bool inside = true;
|
||||
Vector3 maxT;
|
||||
Vector3 coord;
|
||||
double candidatePlane[3];
|
||||
|
||||
// Find candidate planes; this loop can be avoided if rays cast all from the eye(assume perpsective view)
|
||||
for (int i=0; i<3; i++)
|
||||
if(v1.c[i] < min.c[i]) {
|
||||
quadrant[i] = LEFT;
|
||||
candidatePlane[i] = min.c[i];
|
||||
inside = false;
|
||||
} else if(v1.c[i] > max.c[i]) {
|
||||
quadrant[i] = RIGHT;
|
||||
candidatePlane[i] = max.c[i];
|
||||
inside = false;
|
||||
} else {
|
||||
quadrant[i] = MIDDLE;
|
||||
}
|
||||
|
||||
/* Ray v1 inside bounding box */
|
||||
if (inside) {
|
||||
coord = v1;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Calculate T distances to candidate planes */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (quadrant[i] != MIDDLE && dir[i] != 0.0f) {
|
||||
maxT.c[i] = (candidatePlane[i]-v1.c[i]) / dir[i];
|
||||
} else {
|
||||
maxT.c[i] = -1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* Get largest of the maxT's for final choice of intersection */
|
||||
int whichPlane = 0;
|
||||
for (int i = 1; i < 3; i++) {
|
||||
if (maxT.c[whichPlane] < maxT.c[i]) {
|
||||
whichPlane = i;
|
||||
}
|
||||
}
|
||||
|
||||
/* Check final candidate actually inside box */
|
||||
if (maxT.c[whichPlane] < 0.0f) {
|
||||
return false;
|
||||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (whichPlane != i) {
|
||||
coord[i] = v1.c[i] + maxT.c[whichPlane] *dir[i];
|
||||
if (coord[i] < min.c[i] || coord[i] > max.c[i]) {
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
assert(quadrant[i] != MIDDLE); // This should not be possible
|
||||
coord[i] = candidatePlane[i];
|
||||
}
|
||||
}
|
||||
return true; /* ray hits box */
|
||||
}
|
||||
|
||||
bool AABB::intersectsSphere(const Vector3 ¢er, float radius) const
|
||||
{
|
||||
// Arvo's Algorithm
|
||||
|
||||
float squaredDistance = 0;
|
||||
|
||||
// process X
|
||||
if (center.x < min.x) {
|
||||
float diff = center.x - min.x;
|
||||
squaredDistance += diff * diff;
|
||||
} else if (center.x > max.x) {
|
||||
float diff = center.x - max.x;
|
||||
squaredDistance += diff * diff;
|
||||
}
|
||||
|
||||
// process Y
|
||||
if (center.y < min.y) {
|
||||
float diff = center.y - min.y;
|
||||
squaredDistance += diff * diff;
|
||||
} else if (center.y > max.y) {
|
||||
float diff = center.y - max.y;
|
||||
squaredDistance += diff * diff;
|
||||
}
|
||||
|
||||
// process Z
|
||||
if (center.z < min.z) {
|
||||
float diff = center.z - min.z;
|
||||
squaredDistance += diff * diff;
|
||||
} else if (center.z > max.z) {
|
||||
float diff = center.z - max.z;
|
||||
squaredDistance += diff * diff;
|
||||
}
|
||||
|
||||
return squaredDistance <= radius;
|
||||
}
|
||||
|
||||
void AABB::encapsulate(const AABB & b)
|
||||
{
|
||||
if(b.min.x < min.x) min.x = b.min.x;
|
||||
if(b.min.y < min.y) min.y = b.min.y;
|
||||
if(b.min.z < min.z) min.z = b.min.z;
|
||||
|
||||
if(b.max.x > max.x) max.x = b.max.x;
|
||||
if(b.max.y > max.y) max.y = b.max.y;
|
||||
if(b.max.z > max.z) max.z = b.max.z;
|
||||
}
|
||||
|
||||
Vector3 AABB::nearestPoint(const Vector3 & v) const
|
||||
{
|
||||
return Vector3::Create(KRCLAMP(v.x, min.x, max.x), KRCLAMP(v.y, min.y, max.y), KRCLAMP(v.z, min.z, max.z));
|
||||
}
|
||||
|
||||
} // namespace kraken
|
||||
|
||||
Reference in New Issue
Block a user