kraken_engine/hydra
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added Kraken Math files, extracted from Kraken Engine

Browse Source
This commit is contained in:
Kearwood Gilbert
2018-02-26 15:24:21 -08:00
commit 4ceb89c3bd
21 changed files with 3810 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

372
src/aabb.cpp Normal file
View 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 &center, 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

100
src/hitinfo.cpp Normal file
View File

@@ -0,0 +1,100 @@
//
// hitinfo.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"
namespace kraken {
HitInfo::HitInfo()
{
m_position = Vector3::Zero();
m_normal = Vector3::Zero();
m_distance = 0.0f;
m_node = NULL;
}
HitInfo::HitInfo(const Vector3 &position, const Vector3 &normal, const float distance, KRNode *node)
{
m_position = position;
m_normal = normal;
m_distance = distance;
m_node = node;
}
HitInfo::HitInfo(const Vector3 &position, const Vector3 &normal, const float distance)
{
m_position = position;
m_normal = normal;
m_distance = distance;
m_node = NULL;
}
HitInfo::~HitInfo()
{
}
bool HitInfo::didHit() const
{
return m_normal != Vector3::Zero();
}
Vector3 HitInfo::getPosition() const
{
return m_position;
}
Vector3 HitInfo::getNormal() const
{
return m_normal;
}
float HitInfo::getDistance() const
{
return m_distance;
}
KRNode *HitInfo::getNode() const
{
return m_node;
}
HitInfo& HitInfo::operator =(const HitInfo& b)
{
m_position = b.m_position;
m_normal = b.m_normal;
m_distance = b.m_distance;
m_node = b.m_node;
return *this;
}
} // namespace kraken

70
src/krhelpers.cpp Normal file
View File

@@ -0,0 +1,70 @@
//
// krhelpers.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 "krhelpers.h"
#if defined(DEBUG) || defined(_DEBUG)
#define GLDEBUG(x) \
x; \
{ \
GLenum e; \
while( (e=glGetError()) != GL_NO_ERROR) \
{ \
fprintf(stderr, "Error at line number %d, in file %s. glGetError() returned %i for call %s\n",__LINE__, __FILE__, e, #x ); \
} \
}
#else
#define GLDEBUG(x) x;
#endif
namespace kraken {
void SetUniform(GLint location, const Vector2 &v)
{
if (location != -1) GLDEBUG(glUniform2f(location, v.x, v.y));
}
void SetUniform(GLint location, const Vector3 &v)
{
if (location != -1) GLDEBUG(glUniform3f(location, v.x, v.y, v.z));
}
void SetUniform(GLint location, const Vector4 &v)
{
if (location != -1) GLDEBUG(glUniform4f(location, v.x, v.y, v.z, v.w));
}
void SetUniform(GLint location, const Matrix4 &v)
{
if (location != -1) GLDEBUG(glUniformMatrix4fv(location, 1, GL_FALSE, v.c));
}
} // namespace kraken

32
src/krhelpers.h Normal file
View File

@@ -0,0 +1,32 @@
#ifndef KRHELPERS_H
#define KRHELPERS_H
#if defined(_WIN32) || defined(_WIN64)
#include <GL/glew.h>
#elif defined(__linux__) || defined(__unix__) || defined(__posix__)
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glext.h>
#elif defined(__APPLE__)
#include <OpenGL/gl3.h>
#include <OpenGL/gl3ext.h>
#endif
#include "../include/kraken-math.h"
#define KRMIN(x,y) ((x) < (y) ? (x) : (y))
#define KRMAX(x,y) ((x) > (y) ? (x) : (y))
#define KRCLAMP(x, min, max) (KRMAX(KRMIN(x, max), min))
#define KRALIGN(x) ((x + 3) & ~0x03)
float const PI = 3.141592653589793f;
float const D2R = PI * 2 / 360;
namespace kraken {
void SetUniform(GLint location, const Vector2 &v);
void SetUniform(GLint location, const Vector3 &v);
void SetUniform(GLint location, const Vector4 &v);
void SetUniform(GLint location, const Matrix4 &v);
} // namespace kraken
#endif

437
src/matrix4.cpp Normal file
View File

@@ -0,0 +1,437 @@
//
// matrix4.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 <string.h>
namespace kraken {
void Matrix4::init() {
// Default constructor - Initialize with an identity matrix
static const float IDENTITY_MATRIX[] = {
1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0
};
memcpy(c, IDENTITY_MATRIX, sizeof(float) * 16);
}
void Matrix4::init(float *pMat) {
memcpy(c, pMat, sizeof(float) * 16);
}
void Matrix4::init(const Vector3 &new_axis_x, const Vector3 &new_axis_y, const Vector3 &new_axis_z, const Vector3 &new_transform)
{
c[0] = new_axis_x.x; c[1] = new_axis_x.y; c[2] = new_axis_x.z; c[3] = 0.0f;
c[4] = new_axis_y.x; c[5] = new_axis_y.y; c[6] = new_axis_y.z; c[7] = 0.0f;
c[8] = new_axis_z.x; c[9] = new_axis_z.y; c[10] = new_axis_z.z; c[11] = 0.0f;
c[12] = new_transform.x; c[13] = new_transform.y; c[14] = new_transform.z; c[15] = 1.0f;
}
void Matrix4::init(const Matrix4 &m) {
memcpy(c, m.c, sizeof(float) * 16);
}
float *Matrix4::getPointer() {
return c;
}
float& Matrix4::operator[](unsigned i) {
return c[i];
}
float Matrix4::operator[](unsigned i) const {
return c[i];
}
// Overload comparison operator
bool Matrix4::operator==(const Matrix4 &m) const {
return memcmp(c, m.c, sizeof(float) * 16) == 0;
}
// Overload compound multiply operator
Matrix4& Matrix4::operator*=(const Matrix4 &m) {
float temp[16];
int x,y;
for (x=0; x < 4; x++)
{
for(y=0; y < 4; y++)
{
temp[y + (x*4)] = (c[x*4] * m.c[y]) +
(c[(x*4)+1] * m.c[y+4]) +
(c[(x*4)+2] * m.c[y+8]) +
(c[(x*4)+3] * m.c[y+12]);
}
}
memcpy(c, temp, sizeof(float) << 4);
return *this;
}
// Overload multiply operator
Matrix4 Matrix4::operator*(const Matrix4 &m) const {
Matrix4 ret = *this;
ret *= m;
return ret;
}
/* Generate a perspective view matrix using a field of view angle fov,
* window aspect ratio, near and far clipping planes */
void Matrix4::perspective(float fov, float aspect, float nearz, float farz) {
memset(c, 0, sizeof(float) * 16);
float range= tan(fov * 0.5) * nearz;
c[0] = (2 * nearz) / ((range * aspect) - (-range * aspect));
c[5] = (2 * nearz) / (2 * range);
c[10] = -(farz + nearz) / (farz - nearz);
c[11] = -1;
c[14] = -(2 * farz * nearz) / (farz - nearz);
/*
float range= atan(fov / 20.0f) * nearz;
float r = range * aspect;
float t = range * 1.0;
c[0] = nearz / r;
c[5] = nearz / t;
c[10] = -(farz + nearz) / (farz - nearz);
c[11] = -(2.0 * farz * nearz) / (farz - nearz);
c[14] = -1.0;
*/
}
/* Perform translation operations on a matrix */
void Matrix4::translate(float x, float y, float z) {
Matrix4 newMatrix; // Create new identity matrix
newMatrix.c[12] = x;
newMatrix.c[13] = y;
newMatrix.c[14] = z;
*this *= newMatrix;
}
void Matrix4::translate(const Vector3 &v)
{
translate(v.x, v.y, v.z);
}
/* Rotate a matrix by an angle on a X, Y, or Z axis */
void Matrix4::rotate(float angle, AXIS axis) {
const int cos1[3] = { 5, 0, 0 }; // cos(angle)
const int cos2[3] = { 10, 10, 5 }; // cos(angle)
const int sin1[3] = { 9, 2, 4 }; // -sin(angle)
const int sin2[3] = { 6, 8, 1 }; // sin(angle)
/*
X_AXIS:
1, 0, 0, 0
0, cos(angle), -sin(angle), 0
0, sin(angle), cos(angle), 0
0, 0, 0, 1
Y_AXIS:
cos(angle), 0, -sin(angle), 0
0, 1, 0, 0
sin(angle), 0, cos(angle), 0
0, 0, 0, 1
Z_AXIS:
cos(angle), -sin(angle), 0, 0
sin(angle), cos(angle), 0, 0
0, 0, 1, 0
0, 0, 0, 1
*/
Matrix4 newMatrix; // Create new identity matrix
newMatrix.c[cos1[axis]] = cos(angle);
newMatrix.c[sin1[axis]] = -sin(angle);
newMatrix.c[sin2[axis]] = -newMatrix.c[sin1[axis]];
newMatrix.c[cos2[axis]] = newMatrix.c[cos1[axis]];
*this *= newMatrix;
}
void Matrix4::rotate(const Quaternion &q)
{
*this *= q.rotationMatrix();
}
/* Scale matrix by separate x, y, and z amounts */
void Matrix4::scale(float x, float y, float z) {
Matrix4 newMatrix; // Create new identity matrix
newMatrix.c[0] = x;
newMatrix.c[5] = y;
newMatrix.c[10] = z;
*this *= newMatrix;
}
void Matrix4::scale(const Vector3 &v) {
scale(v.x, v.y, v.z);
}
/* Scale all dimensions equally */
void Matrix4::scale(float s) {
scale(s,s,s);
}
// Initialize with a bias matrix
void Matrix4::bias() {
static const float BIAS_MATRIX[] = {
0.5, 0.0, 0.0, 0.0,
0.0, 0.5, 0.0, 0.0,
0.0, 0.0, 0.5, 0.0,
0.5, 0.5, 0.5, 1.0
};
memcpy(c, BIAS_MATRIX, sizeof(float) * 16);
}
/* Generate an orthographic view matrix */
void Matrix4::ortho(float left, float right, float top, float bottom, float nearz, float farz) {
memset(c, 0, sizeof(float) * 16);
c[0] = 2.0f / (right - left);
c[5] = 2.0f / (bottom - top);
c[10] = -1.0f / (farz - nearz);
c[11] = -nearz / (farz - nearz);
c[15] = 1.0f;
}
/* Replace matrix with its inverse */
bool Matrix4::invert() {
// Based on gluInvertMatrix implementation
float inv[16], det;
int i;
inv[0] = c[5]*c[10]*c[15] - c[5]*c[11]*c[14] - c[9]*c[6]*c[15]
+ c[9]*c[7]*c[14] + c[13]*c[6]*c[11] - c[13]*c[7]*c[10];
inv[4] = -c[4]*c[10]*c[15] + c[4]*c[11]*c[14] + c[8]*c[6]*c[15]
- c[8]*c[7]*c[14] - c[12]*c[6]*c[11] + c[12]*c[7]*c[10];
inv[8] = c[4]*c[9]*c[15] - c[4]*c[11]*c[13] - c[8]*c[5]*c[15]
+ c[8]*c[7]*c[13] + c[12]*c[5]*c[11] - c[12]*c[7]*c[9];
inv[12] = -c[4]*c[9]*c[14] + c[4]*c[10]*c[13] + c[8]*c[5]*c[14]
- c[8]*c[6]*c[13] - c[12]*c[5]*c[10] + c[12]*c[6]*c[9];
inv[1] = -c[1]*c[10]*c[15] + c[1]*c[11]*c[14] + c[9]*c[2]*c[15]
- c[9]*c[3]*c[14] - c[13]*c[2]*c[11] + c[13]*c[3]*c[10];
inv[5] = c[0]*c[10]*c[15] - c[0]*c[11]*c[14] - c[8]*c[2]*c[15]
+ c[8]*c[3]*c[14] + c[12]*c[2]*c[11] - c[12]*c[3]*c[10];
inv[9] = -c[0]*c[9]*c[15] + c[0]*c[11]*c[13] + c[8]*c[1]*c[15]
- c[8]*c[3]*c[13] - c[12]*c[1]*c[11] + c[12]*c[3]*c[9];
inv[13] = c[0]*c[9]*c[14] - c[0]*c[10]*c[13] - c[8]*c[1]*c[14]
+ c[8]*c[2]*c[13] + c[12]*c[1]*c[10] - c[12]*c[2]*c[9];
inv[2] = c[1]*c[6]*c[15] - c[1]*c[7]*c[14] - c[5]*c[2]*c[15]
+ c[5]*c[3]*c[14] + c[13]*c[2]*c[7] - c[13]*c[3]*c[6];
inv[6] = -c[0]*c[6]*c[15] + c[0]*c[7]*c[14] + c[4]*c[2]*c[15]
- c[4]*c[3]*c[14] - c[12]*c[2]*c[7] + c[12]*c[3]*c[6];
inv[10] = c[0]*c[5]*c[15] - c[0]*c[7]*c[13] - c[4]*c[1]*c[15]
+ c[4]*c[3]*c[13] + c[12]*c[1]*c[7] - c[12]*c[3]*c[5];
inv[14] = -c[0]*c[5]*c[14] + c[0]*c[6]*c[13] + c[4]*c[1]*c[14]
- c[4]*c[2]*c[13] - c[12]*c[1]*c[6] + c[12]*c[2]*c[5];
inv[3] = -c[1]*c[6]*c[11] + c[1]*c[7]*c[10] + c[5]*c[2]*c[11]
- c[5]*c[3]*c[10] - c[9]*c[2]*c[7] + c[9]*c[3]*c[6];
inv[7] = c[0]*c[6]*c[11] - c[0]*c[7]*c[10] - c[4]*c[2]*c[11]
+ c[4]*c[3]*c[10] + c[8]*c[2]*c[7] - c[8]*c[3]*c[6];
inv[11] = -c[0]*c[5]*c[11] + c[0]*c[7]*c[9] + c[4]*c[1]*c[11]
- c[4]*c[3]*c[9] - c[8]*c[1]*c[7] + c[8]*c[3]*c[5];
inv[15] = c[0]*c[5]*c[10] - c[0]*c[6]*c[9] - c[4]*c[1]*c[10]
+ c[4]*c[2]*c[9] + c[8]*c[1]*c[6] - c[8]*c[2]*c[5];
det = c[0]*inv[0] + c[1]*inv[4] + c[2]*inv[8] + c[3]*inv[12];
if (det == 0) {
return false;
}
det = 1.0 / det;
for (i = 0; i < 16; i++) {
c[i] = inv[i] * det;
}
return true;
}
void Matrix4::transpose() {
float trans[16];
for(int x=0; x<4; x++) {
for(int y=0; y<4; y++) {
trans[x + y * 4] = c[y + x * 4];
}
}
memcpy(c, trans, sizeof(float) * 16);
}
/* Dot Product, returning Vector3 */
Vector3 Matrix4::Dot(const Matrix4 &m, const Vector3 &v) {
return Vector3::Create(
v.c[0] * m.c[0] + v.c[1] * m.c[4] + v.c[2] * m.c[8] + m.c[12],
v.c[0] * m.c[1] + v.c[1] * m.c[5] + v.c[2] * m.c[9] + m.c[13],
v.c[0] * m.c[2] + v.c[1] * m.c[6] + v.c[2] * m.c[10] + m.c[14]
);
}
Vector4 Matrix4::Dot4(const Matrix4 &m, const Vector4 &v) {
#ifdef KRAKEN_USE_ARM_NEON
Vector4 d;
asm volatile (
"vld1.32 {d0, d1}, [%1] \n\t" //Q0 = v
"vld1.32 {d18, d19}, [%0]! \n\t" //Q1 = m
"vld1.32 {d20, d21}, [%0]! \n\t" //Q2 = m+4
"vld1.32 {d22, d23}, [%0]! \n\t" //Q3 = m+8
"vld1.32 {d24, d25}, [%0]! \n\t" //Q4 = m+12
"vmul.f32 q13, q9, d0[0] \n\t" //Q5 = Q1*Q0[0]
"vmla.f32 q13, q10, d0[1] \n\t" //Q5 += Q1*Q0[1]
"vmla.f32 q13, q11, d1[0] \n\t" //Q5 += Q2*Q0[2]
"vmla.f32 q13, q12, d1[1] \n\t" //Q5 += Q3*Q0[3]
"vst1.32 {d26, d27}, [%2] \n\t" //Q4 = m+12
: /* no output registers */
: "r"(m.c), "r"(v.c), "r"(d.c)
: "q0", "q9", "q10","q11", "q12", "q13", "memory"
);
return d;
#else
return Vector4::Create(
v.c[0] * m.c[0] + v.c[1] * m.c[4] + v.c[2] * m.c[8] + m.c[12],
v.c[0] * m.c[1] + v.c[1] * m.c[5] + v.c[2] * m.c[9] + m.c[13],
v.c[0] * m.c[2] + v.c[1] * m.c[6] + v.c[2] * m.c[10] + m.c[14],
v.c[0] * m.c[3] + v.c[1] * m.c[7] + v.c[2] * m.c[11] + m.c[15]
);
#endif
}
// Dot product without including translation; useful for transforming normals and tangents
Vector3 Matrix4::DotNoTranslate(const Matrix4 &m, const Vector3 &v)
{
return Vector3::Create(
v.x * m.c[0] + v.y * m.c[4] + v.z * m.c[8],
v.x * m.c[1] + v.y * m.c[5] + v.z * m.c[9],
v.x * m.c[2] + v.y * m.c[6] + v.z * m.c[10]
);
}
/* Dot Product, returning w component as if it were a Vector4 (This will be deprecated once Vector4 is implemented instead*/
float Matrix4::DotW(const Matrix4 &m, const Vector3 &v) {
return v.x * m.c[0*4 + 3] + v.y * m.c[1*4 + 3] + v.z * m.c[2*4 + 3] + m.c[3*4 + 3];
}
/* Dot Product followed by W-divide */
Vector3 Matrix4::DotWDiv(const Matrix4 &m, const Vector3 &v) {
Vector4 r = Dot4(m, Vector4::Create(v, 1.0f));
return Vector3::Create(r) / r.w;
}
Matrix4 Matrix4::LookAt(const Vector3 &cameraPos, const Vector3 &lookAtPos, const Vector3 &upDirection)
{
Matrix4 matLookat;
Vector3 lookat_z_axis = lookAtPos - cameraPos;
lookat_z_axis.normalize();
Vector3 lookat_x_axis = Vector3::Cross(upDirection, lookat_z_axis);
lookat_x_axis.normalize();
Vector3 lookat_y_axis = Vector3::Cross(lookat_z_axis, lookat_x_axis);
matLookat.getPointer()[0] = lookat_x_axis.x;
matLookat.getPointer()[1] = lookat_y_axis.x;
matLookat.getPointer()[2] = lookat_z_axis.x;
matLookat.getPointer()[4] = lookat_x_axis.y;
matLookat.getPointer()[5] = lookat_y_axis.y;
matLookat.getPointer()[6] = lookat_z_axis.y;
matLookat.getPointer()[8] = lookat_x_axis.z;
matLookat.getPointer()[9] = lookat_y_axis.z;
matLookat.getPointer()[10] = lookat_z_axis.z;
matLookat.getPointer()[12] = -Vector3::Dot(lookat_x_axis, cameraPos);
matLookat.getPointer()[13] = -Vector3::Dot(lookat_y_axis, cameraPos);
matLookat.getPointer()[14] = -Vector3::Dot(lookat_z_axis, cameraPos);
return matLookat;
}
Matrix4 Matrix4::Invert(const Matrix4 &m)
{
Matrix4 matInvert = m;
matInvert.invert();
return matInvert;
}
Matrix4 Matrix4::Transpose(const Matrix4 &m)
{
Matrix4 matTranspose = m;
matTranspose.transpose();
return matTranspose;
}
Matrix4 Matrix4::Translation(const Vector3 &v)
{
Matrix4 m;
m[12] = v.x;
m[13] = v.y;
m[14] = v.z;
// m.translate(v);
return m;
}
Matrix4 Matrix4::Rotation(const Vector3 &v)
{
Matrix4 m;
m.rotate(v.x, X_AXIS);
m.rotate(v.y, Y_AXIS);
m.rotate(v.z, Z_AXIS);
return m;
}
Matrix4 Matrix4::Scaling(const Vector3 &v)
{
Matrix4 m;
m.scale(v);
return m;
}
} // namespace kraken

388
src/quaternion.cpp Normal file
View File

@@ -0,0 +1,388 @@
//
// quaternion.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 "krhelpers.h"
namespace kraken {
void Quaternion::init() {
c[0] = 1.0;
c[1] = 0.0;
c[2] = 0.0;
c[3] = 0.0;
}
Quaternion Quaternion::Create()
{
Quaternion r;
r.init();
return r;
}
void Quaternion::init(float w, float x, float y, float z) {
c[0] = w;
c[1] = x;
c[2] = y;
c[3] = z;
}
Quaternion Quaternion::Create(float w, float x, float y, float z)
{
Quaternion r;
r.init(w, x, y, z);
return r;
}
void Quaternion::init(const Quaternion& p) {
c[0] = p[0];
c[1] = p[1];
c[2] = p[2];
c[3] = p[3];
}
Quaternion Quaternion::Create(const Quaternion& p)
{
Quaternion r;
r.init(p);
return r;
}
void Quaternion::init(const Vector3 &euler) {
setEulerZYX(euler);
}
Quaternion Quaternion::Create(const Vector3 &euler)
{
Quaternion r;
r.init(euler);
return r;
}
void Quaternion::init(const Vector3 &from_vector, const Vector3 &to_vector) {
Vector3 a = Vector3::Cross(from_vector, to_vector);
c[0] = a[0];
c[1] = a[1];
c[2] = a[2];
c[3] = sqrt(from_vector.sqrMagnitude() * to_vector.sqrMagnitude()) + Vector3::Dot(from_vector, to_vector);
normalize();
}
Quaternion Quaternion::Create(const Vector3 &from_vector, const Vector3 &to_vector)
{
Quaternion r;
r.init(from_vector, to_vector);
return r;
}
void Quaternion::setEulerXYZ(const Vector3 &euler)
{
*this = Quaternion::FromAngleAxis(Vector3::Create(1.0f, 0.0f, 0.0f), euler.x)
* Quaternion::FromAngleAxis(Vector3::Create(0.0f, 1.0f, 0.0f), euler.y)
* Quaternion::FromAngleAxis(Vector3::Create(0.0f, 0.0f, 1.0f), euler.z);
}
void Quaternion::setEulerZYX(const Vector3 &euler) {
// ZYX Order!
float c1 = cos(euler[0] * 0.5f);
float c2 = cos(euler[1] * 0.5f);
float c3 = cos(euler[2] * 0.5f);
float s1 = sin(euler[0] * 0.5f);
float s2 = sin(euler[1] * 0.5f);
float s3 = sin(euler[2] * 0.5f);
c[0] = c1 * c2 * c3 + s1 * s2 * s3;
c[1] = s1 * c2 * c3 - c1 * s2 * s3;
c[2] = c1 * s2 * c3 + s1 * c2 * s3;
c[3] = c1 * c2 * s3 - s1 * s2 * c3;
}
float Quaternion::operator [](unsigned i) const {
return c[i];
}
float &Quaternion::operator [](unsigned i) {
return c[i];
}
Vector3 Quaternion::eulerXYZ() const {
double a2 = 2 * (c[0] * c[2] - c[1] * c[3]);
if(a2 <= -0.99999) {
return Vector3::Create(
2.0 * atan2(c[1], c[0]),
-PI * 0.5f,
0
);
} else if(a2 >= 0.99999) {
return Vector3::Create(
2.0 * atan2(c[1], c[0]),
PI * 0.5f,
0
);
} else {
return Vector3::Create(
atan2(2 * (c[0] * c[1] + c[2] * c[3]), (1 - 2 * (c[1] * c[1] + c[2] * c[2]))),
asin(a2),
atan2(2 * (c[0] * c[3] + c[1] * c[2]), (1 - 2 * (c[2] * c[2] + c[3] * c[3])))
);
}
}
bool operator ==(Quaternion &v1, Quaternion &v2) {
return
v1[0] == v2[0]
&& v1[1] == v2[1]
&& v1[2] == v2[2]
&& v1[3] == v2[3];
}
bool operator !=(Quaternion &v1, Quaternion &v2) {
return
v1[0] != v2[0]
|| v1[1] != v2[1]
|| v1[2] != v2[2]
|| v1[3] != v2[3];
}
Quaternion Quaternion::operator *(const Quaternion &v) {
float t0 = (c[3]-c[2])*(v[2]-v[3]);
float t1 = (c[0]+c[1])*(v[0]+v[1]);
float t2 = (c[0]-c[1])*(v[2]+v[3]);
float t3 = (c[3]+c[2])*(v[0]-v[1]);
float t4 = (c[3]-c[1])*(v[1]-v[2]);
float t5 = (c[3]+c[1])*(v[1]+v[2]);
float t6 = (c[0]+c[2])*(v[0]-v[3]);
float t7 = (c[0]-c[2])*(v[0]+v[3]);
float t8 = t5+t6+t7;
float t9 = (t4+t8)/2;
return Quaternion::Create(
t0+t9-t5,
t1+t9-t8,
t2+t9-t7,
t3+t9-t6
);
}
Quaternion Quaternion::operator *(float v) const {
return Quaternion::Create(c[0] * v, c[1] * v, c[2] * v, c[3] * v);
}
Quaternion Quaternion::operator /(float num) const {
float inv_num = 1.0f / num;
return Quaternion::Create(c[0] * inv_num, c[1] * inv_num, c[2] * inv_num, c[3] * inv_num);
}
Quaternion Quaternion::operator +(const Quaternion &v) const {
return Quaternion::Create(c[0] + v[0], c[1] + v[1], c[2] + v[2], c[3] + v[3]);
}
Quaternion Quaternion::operator -(const Quaternion &v) const {
return Quaternion::Create(c[0] - v[0], c[1] - v[1], c[2] - v[2], c[3] - v[3]);
}
Quaternion& Quaternion::operator +=(const Quaternion& v) {
c[0] += v[0];
c[1] += v[1];
c[2] += v[2];
c[3] += v[3];
return *this;
}
Quaternion& Quaternion::operator -=(const Quaternion& v) {
c[0] -= v[0];
c[1] -= v[1];
c[2] -= v[2];
c[3] -= v[3];
return *this;
}
Quaternion& Quaternion::operator *=(const Quaternion& v) {
float t0 = (c[3]-c[2])*(v[2]-v[3]);
float t1 = (c[0]+c[1])*(v[0]+v[1]);
float t2 = (c[0]-c[1])*(v[2]+v[3]);
float t3 = (c[3]+c[2])*(v[0]-v[1]);
float t4 = (c[3]-c[1])*(v[1]-v[2]);
float t5 = (c[3]+c[1])*(v[1]+v[2]);
float t6 = (c[0]+c[2])*(v[0]-v[3]);
float t7 = (c[0]-c[2])*(v[0]+v[3]);
float t8 = t5+t6+t7;
float t9 = (t4+t8)/2;
c[0] = t0+t9-t5;
c[1] = t1+t9-t8;
c[2] = t2+t9-t7;
c[3] = t3+t9-t6;
return *this;
}
Quaternion& Quaternion::operator *=(const float& v) {
c[0] *= v;
c[1] *= v;
c[2] *= v;
c[3] *= v;
return *this;
}
Quaternion& Quaternion::operator /=(const float& v) {
float inv_v = 1.0f / v;
c[0] *= inv_v;
c[1] *= inv_v;
c[2] *= inv_v;
c[3] *= inv_v;
return *this;
}
Quaternion Quaternion::operator +() const {
return *this;
}
Quaternion Quaternion::operator -() const {
return Quaternion::Create(-c[0], -c[1], -c[2], -c[3]);
}
Quaternion Normalize(const Quaternion &v1) {
float inv_magnitude = 1.0f / sqrtf(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3]);
return Quaternion::Create(
v1[0] * inv_magnitude,
v1[1] * inv_magnitude,
v1[2] * inv_magnitude,
v1[3] * inv_magnitude
);
}
void Quaternion::normalize() {
float inv_magnitude = 1.0f / sqrtf(c[0] * c[0] + c[1] * c[1] + c[2] * c[2] + c[3] * c[3]);
c[0] *= inv_magnitude;
c[1] *= inv_magnitude;
c[2] *= inv_magnitude;
c[3] *= inv_magnitude;
}
Quaternion Conjugate(const Quaternion &v1) {
return Quaternion::Create(v1[0], -v1[1], -v1[2], -v1[3]);
}
void Quaternion::conjugate() {
c[1] = -c[1];
c[2] = -c[2];
c[3] = -c[3];
}
Matrix4 Quaternion::rotationMatrix() const {
Matrix4 matRotate;
/*
Vector3 euler = eulerXYZ();
matRotate.rotate(euler.x, X_AXIS);
matRotate.rotate(euler.y, Y_AXIS);
matRotate.rotate(euler.z, Z_AXIS);
*/
// FINDME - Determine why the more optimal routine commented below wasn't working
matRotate.c[0] = 1.0 - 2.0 * (c[2] * c[2] + c[3] * c[3]);
matRotate.c[1] = 2.0 * (c[1] * c[2] - c[0] * c[3]);
matRotate.c[2] = 2.0 * (c[0] * c[2] + c[1] * c[3]);
matRotate.c[4] = 2.0 * (c[1] * c[2] + c[0] * c[3]);
matRotate.c[5] = 1.0 - 2.0 * (c[1] * c[1] + c[3] * c[3]);
matRotate.c[6] = 2.0 * (c[2] * c[3] - c[0] * c[1]);
matRotate.c[8] = 2.0 * (c[1] * c[3] - c[0] * c[2]);
matRotate.c[9] = 2.0 * (c[0] * c[1] + c[2] * c[3]);
matRotate.c[10] = 1.0 - 2.0 * (c[1] * c[1] + c[2] * c[2]);
return matRotate;
}
Quaternion Quaternion::FromAngleAxis(const Vector3 &axis, float angle)
{
float ha = angle * 0.5f;
float sha = sin(ha);
return Quaternion::Create(cos(ha), axis.x * sha, axis.y * sha, axis.z * sha);
}
float Quaternion::Dot(const Quaternion &v1, const Quaternion &v2)
{
return v1.c[0] * v2.c[0] + v1.c[1] * v2.c[1] + v1.c[2] * v2.c[2] + v1.c[3] * v2.c[3];
}
Quaternion Quaternion::Lerp(const Quaternion &a, const Quaternion &b, float t)
{
if (t <= 0.0f) {
return a;
} else if (t >= 1.0f) {
return b;
}
return a * (1.0f - t) + b * t;
}
Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b, float t)
{
if (t <= 0.0f) {
return a;
}
if (t >= 1.0f) {
return b;
}
float coshalftheta = Dot(a, b);
Quaternion c = a;
// Angle is greater than 180. We can negate the angle/quat to get the
// shorter rotation to reach the same destination.
if ( coshalftheta < 0.0f ) {
coshalftheta = -coshalftheta;
c = -c;
}
if ( coshalftheta > (1.0f - std::numeric_limits<float>::epsilon())) {
// Angle is tiny - save some computation by lerping instead.
return Lerp(c, b, t);
}
float halftheta = acos(coshalftheta);
return (c * sin((1.0f - t) * halftheta) + b * sin(t * halftheta)) / sin(halftheta);
}
} // namespace kraken

42
src/scalar.cpp Normal file
View File

@@ -0,0 +1,42 @@
//
// scalar.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"
namespace kraken {
float SmoothStep(float a, float b, float t)
{
float d = (3.0 * t * t - 2.0 * t * t * t);
return a * (1.0f - d) + b * d;
}
} // namespace kraken

340
src/triangle3.cpp Normal file
View File

@@ -0,0 +1,340 @@
//
// KRTriangle.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"
using namespace kraken;
namespace {
bool _intersectSphere(const Vector3 &start, const Vector3 &dir, const Vector3 &sphere_center, float sphere_radius, float &distance)
{
// dir must be normalized
// From: http://archive.gamedev.net/archive/reference/articles/article1026.html
// TODO - Move to another class?
Vector3 Q = sphere_center - start;
float c = Q.magnitude();
float v = Vector3::Dot(Q, dir);
float d = sphere_radius * sphere_radius - (c * c - v * v);
if (d < 0.0) {
// No intersection
return false;
}
// Return the distance to the [first] intersecting point
distance = v - sqrt(d);
if (distance < 0.0f) {
return false;
}
return true;
}
bool _sameSide(const Vector3 &p1, const Vector3 &p2, const Vector3 &a, const Vector3 &b)
{
// TODO - Move to Vector3 class?
// From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle
Vector3 cp1 = Vector3::Cross(b - a, p1 - a);
Vector3 cp2 = Vector3::Cross(b - a, p2 - a);
if (Vector3::Dot(cp1, cp2) >= 0) return true;
return false;
}
Vector3 _closestPointOnLine(const Vector3 &a, const Vector3 &b, const Vector3 &p)
{
// From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle
// Determine t (the length of the vector from ‘a’ to ‘p’)
Vector3 c = p - a;
Vector3 V = Vector3::Normalize(b - a);
float d = (a - b).magnitude();
float t = Vector3::Dot(V, c);
// Check to see if ‘t’ is beyond the extents of the line segment
if (t < 0) return a;
if (t > d) return b;
// Return the point between ‘a’ and ‘b’
return a + V * t;
}
} // anonymous namespace
namespace kraken {
void Triangle3::init(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
{
vert[0] = v1;
vert[1] = v2;
vert[2] = v3;
}
void Triangle3::init(const Triangle3 &tri)
{
vert[0] = tri[0];
vert[1] = tri[1];
vert[3] = tri[3];
}
bool Triangle3::operator ==(const Triangle3& b) const
{
return vert[0] == b[0] && vert[1] == b[1] && vert[2] == b[2];
}
bool Triangle3::operator !=(const Triangle3& b) const
{
return vert[0] != b[0] || vert[1] != b[1] || vert[2] != b[2];
}
Vector3& Triangle3::operator[](unsigned int i)
{
return vert[i];
}
Vector3 Triangle3::operator[](unsigned int i) const
{
return vert[i];
}
bool Triangle3::rayCast(const Vector3 &start, const Vector3 &dir, Vector3 &hit_point) const
{
// algorithm based on Dan Sunday's implementation at http://geomalgorithms.com/a06-_intersect-2.html
const float SMALL_NUM = 0.00000001; // anything that avoids division overflow
Vector3 u, v, n; // triangle vectors
Vector3 w0, w; // ray vectors
float r, a, b; // params to calc ray-plane intersect
// get triangle edge vectors and plane normal
u = vert[1] - vert[0];
v = vert[2] - vert[0];
n = Vector3::Cross(u, v); // cross product
if (n == Vector3::Zero()) // triangle is degenerate
return false; // do not deal with this case
w0 = start - vert[0];
a = -Vector3::Dot(n, w0);
b = Vector3::Dot(n,dir);
if (fabs(b) < SMALL_NUM) { // ray is parallel to triangle plane
if (a == 0)
return false; // ray lies in triangle plane
else {
return false; // ray disjoint from plane
}
}
// get intersect point of ray with triangle plane
r = a / b;
if (r < 0.0) // ray goes away from triangle
return false; // => no intersect
// for a segment, also test if (r > 1.0) => no intersect
Vector3 plane_hit_point = start + dir * r; // intersect point of ray and plane
// is plane_hit_point inside triangle?
float uu, uv, vv, wu, wv, D;
uu = Vector3::Dot(u,u);
uv = Vector3::Dot(u,v);
vv = Vector3::Dot(v,v);
w = plane_hit_point - vert[0];
wu = Vector3::Dot(w,u);
wv = Vector3::Dot(w,v);
D = uv * uv - uu * vv;
// get and test parametric coords
float s, t;
s = (uv * wv - vv * wu) / D;
if (s < 0.0 || s > 1.0) // plane_hit_point is outside triangle
return false;
t = (uv * wu - uu * wv) / D;
if (t < 0.0 || (s + t) > 1.0) // plane_hit_point is outside triangle
return false;
// plane_hit_point is inside the triangle
hit_point = plane_hit_point;
return true;
}
Vector3 Triangle3::calculateNormal() const
{
Vector3 v1 = vert[1] - vert[0];
Vector3 v2 = vert[2] - vert[0];
return Vector3::Normalize(Vector3::Cross(v1, v2));
}
Vector3 Triangle3::closestPointOnTriangle(const Vector3 &p) const
{
Vector3 a = vert[0];
Vector3 b = vert[1];
Vector3 c = vert[2];
Vector3 Rab = _closestPointOnLine(a, b, p);
Vector3 Rbc = _closestPointOnLine(b, c, p);
Vector3 Rca = _closestPointOnLine(c, a, p);
// return closest [Rab, Rbc, Rca] to p;
float sd_Rab = (p - Rab).sqrMagnitude();
float sd_Rbc = (p - Rbc).sqrMagnitude();
float sd_Rca = (p - Rca).sqrMagnitude();
if(sd_Rab < sd_Rbc && sd_Rab < sd_Rca) {
return Rab;
} else if(sd_Rbc < sd_Rab && sd_Rbc < sd_Rca) {
return Rbc;
} else {
return Rca;
}
}
bool Triangle3::sphereCast(const Vector3 &start, const Vector3 &dir, float radius, Vector3 &hit_point, float &hit_distance) const
{
// Dir must be normalized
const float SMALL_NUM = 0.001f; // anything that avoids division overflow
Vector3 tri_normal = calculateNormal();
float d = Vector3::Dot(tri_normal, vert[0]);
float e = Vector3::Dot(tri_normal, start) - radius;
float cotangent_distance = e - d;
Vector3 plane_intersect;
float plane_intersect_distance;
float denom = Vector3::Dot(tri_normal, dir);
if(denom > -SMALL_NUM) {
return false; // dir is co-planar with the triangle or going in the direction of the normal; no intersection
}
// Detect an embedded plane, caused by a sphere that is already intersecting the plane.
if(cotangent_distance <= 0 && cotangent_distance >= -radius * 2.0f) {
// Embedded plane - Sphere is already intersecting the plane.
// Use the point closest to the origin of the sphere as the intersection
plane_intersect = start - tri_normal * (cotangent_distance + radius);
plane_intersect_distance = 0.0f;
} else {
// Sphere is not intersecting the plane
// Determine the first point hit by the swept sphere on the triangle's plane
plane_intersect_distance = -(cotangent_distance / denom);
plane_intersect = start + dir * plane_intersect_distance - tri_normal * radius;
}
if(plane_intersect_distance < 0.0f) {
return false;
}
if(containsPoint(plane_intersect)) {
// Triangle contains point
hit_point = plane_intersect;
hit_distance = plane_intersect_distance;
return true;
} else {
// Triangle does not contain point, cast ray back to sphere from closest point on triangle edge or vertice
Vector3 closest_point = closestPointOnTriangle(plane_intersect);
float reverse_hit_distance;
if(_intersectSphere(closest_point, -dir, start, radius, reverse_hit_distance)) {
// Reverse cast hit sphere
hit_distance = reverse_hit_distance;
hit_point = closest_point;
return true;
} else {
// Reverse cast did not hit sphere
return false;
}
}
}
bool Triangle3::containsPoint(const Vector3 &p) const
{
/*
// From: http://stackoverflow.com/questions/995445/determine-if-a-3d-point-is-within-a-triangle
const float SMALL_NUM = 0.00000001f; // anything that avoids division overflow
// Vector3 A = vert[0], B = vert[1], C = vert[2];
if (_sameSide(p, vert[0], vert[1], vert[2]) && _sameSide(p, vert[1], vert[0], vert[2]) && _sameSide(p, vert[2], vert[0], vert[1])) {
Vector3 vc1 = Vector3::Cross(vert[0] - vert[1], vert[0] - vert[2]);
if(fabs(Vector3::Dot(vert[0] - p, vc1)) <= SMALL_NUM) {
return true;
}
}
return false;
*/
// From: http://blogs.msdn.com/b/rezanour/archive/2011/08/07/barycentric-coordinates-and-point-in-triangle-tests.aspx
Vector3 A = vert[0];
Vector3 B = vert[1];
Vector3 C = vert[2];
Vector3 P = p;
// Prepare our barycentric variables
Vector3 u = B - A;
Vector3 v = C - A;
Vector3 w = P - A;
Vector3 vCrossW = Vector3::Cross(v, w);
Vector3 vCrossU = Vector3::Cross(v, u);
// Test sign of r
if (Vector3::Dot(vCrossW, vCrossU) < 0)
return false;
Vector3 uCrossW = Vector3::Cross(u, w);
Vector3 uCrossV = Vector3::Cross(u, v);
// Test sign of t
if (Vector3::Dot(uCrossW, uCrossV) < 0)
return false;
// At this point, we know that r and t and both > 0.
// Therefore, as long as their sum is <= 1, each must be less <= 1
float denom = uCrossV.magnitude();
float r = vCrossW.magnitude() / denom;
float t = uCrossW.magnitude() / denom;
return (r + t <= 1);
}
} // namespace kraken

262
src/vector2.cpp Normal file
View File

@@ -0,0 +1,262 @@
//
// Vector2.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"
namespace kraken {
void Vector2::init() {
x = 0.0;
y = 0.0;
}
Vector2 Vector2::Create()
{
Vector2 r;
r.init();
return r;
}
void Vector2::init(float X, float Y) {
x = X;
y = Y;
}
Vector2 Vector2::Create(float X, float Y)
{
Vector2 r;
r.init(X,Y);
return r;
}
void Vector2::init(float v) {
x = v;
y = v;
}
Vector2 Vector2::Create(float v)
{
Vector2 r;
r.init(v);
return r;
}
void Vector2::init(float *v) {
x = v[0];
y = v[1];
}
Vector2 Vector2::Create(float *v)
{
Vector2 r;
r.init(v);
return r;
}
void Vector2::init(const Vector2 &v) {
x = v.x;
y = v.y;
}
Vector2 Vector2::Create(const Vector2 &v)
{
Vector2 r;
r.init(v);
return r;
}
// Vector2 swizzle getters
Vector2 Vector2::yx() const
{
return Vector2::Create(y,x);
}
// Vector2 swizzle setters
void Vector2::yx(const Vector2 &v)
{
y = v.x;
x = v.y;
}
Vector2 Vector2::Min() {
return Vector2::Create(-std::numeric_limits<float>::max());
}
Vector2 Vector2::Max() {
return Vector2::Create(std::numeric_limits<float>::max());
}
Vector2 Vector2::Zero() {
return Vector2::Create(0.0f);
}
Vector2 Vector2::One() {
return Vector2::Create(1.0f);
}
Vector2 Vector2::operator +(const Vector2& b) const {
return Vector2::Create(x + b.x, y + b.y);
}
Vector2 Vector2::operator -(const Vector2& b) const {
return Vector2::Create(x - b.x, y - b.y);
}
Vector2 Vector2::operator +() const {
return *this;
}
Vector2 Vector2::operator -() const {
return Vector2::Create(-x, -y);
}
Vector2 Vector2::operator *(const float v) const {
return Vector2::Create(x * v, y * v);
}
Vector2 Vector2::operator /(const float v) const {
float inv_v = 1.0f / v;
return Vector2::Create(x * inv_v, y * inv_v);
}
Vector2& Vector2::operator +=(const Vector2& b) {
x += b.x;
y += b.y;
return *this;
}
Vector2& Vector2::operator -=(const Vector2& b) {
x -= b.x;
y -= b.y;
return *this;
}
Vector2& Vector2::operator *=(const float v) {
x *= v;
y *= v;
return *this;
}
Vector2& Vector2::operator /=(const float v) {
float inv_v = 1.0f / v;
x *= inv_v;
y *= inv_v;
return *this;
}
bool Vector2::operator ==(const Vector2& b) const {
return x == b.x && y == b.y;
}
bool Vector2::operator !=(const Vector2& b) const {
return x != b.x || y != b.y;
}
bool Vector2::operator >(const Vector2& b) const
{
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
if(x > b.x) {
return true;
} else if(x < b.x) {
return false;
} else if(y > b.y) {
return true;
} else {
return false;
}
}
bool Vector2::operator <(const Vector2& b) const
{
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
if(x < b.x) {
return true;
} else if(x > b.x) {
return false;
} else if(y < b.y) {
return true;
} else {
return false;
}
}
float& Vector2::operator[] (unsigned i) {
switch(i) {
case 0:
return x;
case 1:
default:
return y;
}
}
float Vector2::operator[](unsigned i) const {
switch(i) {
case 0:
return x;
case 1:
default:
return y;
}
}
void Vector2::normalize() {
float inv_magnitude = 1.0f / sqrtf(x * x + y * y);
x *= inv_magnitude;
y *= inv_magnitude;
}
float Vector2::sqrMagnitude() const {
return x * x + y * y;
}
float Vector2::magnitude() const {
return sqrtf(x * x + y * y);
}
Vector2 Vector2::Normalize(const Vector2 &v) {
float inv_magnitude = 1.0f / sqrtf(v.x * v.x + v.y * v.y);
return Vector2::Create(v.x * inv_magnitude, v.y * inv_magnitude);
}
float Vector2::Cross(const Vector2 &v1, const Vector2 &v2) {
return v1.x * v2.y - v1.y * v2.x;
}
float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) {
return v1.x * v2.x + v1.y * v2.y;
}
} // namepsace kraken

456
src/vector3.cpp Normal file
View File

@@ -0,0 +1,456 @@
//
// Vector3.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"
namespace kraken {
//default constructor
void Vector3::init()
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Vector3 Vector3::Create()
{
Vector3 r;
r.init();
return r;
}
void Vector3::init(const Vector3 &v) {
x = v.x;
y = v.y;
z = v.z;
}
Vector3 Vector3::Create(const Vector3 &v)
{
Vector3 r;
r.init(v);
return r;
}
void Vector3::init(const Vector4 &v) {
x = v.x;
y = v.y;
z = v.z;
}
Vector3 Vector3::Create(const Vector4 &v)
{
Vector3 r;
r.init(v);
return r;
}
void Vector3::init(float *v) {
x = v[0];
y = v[1];
z = v[2];
}
Vector3 Vector3::Create(float *v)
{
Vector3 r;
r.init(v);
return r;
}
void Vector3::init(double *v) {
x = (float)v[0];
y = (float)v[1];
z = (float)v[2];
}
Vector3 Vector3::Create(double *v)
{
Vector3 r;
r.init(v);
return r;
}
void Vector3::init(float v) {
x = v;
y = v;
z = v;
}
Vector3 Vector3::Create(float v)
{
Vector3 r;
r.init(v);
return r;
}
void Vector3::init(float X, float Y, float Z)
{
x = X;
y = Y;
z = Z;
}
Vector3 Vector3::Create(float X, float Y, float Z)
{
Vector3 r;
r.init(X,Y,Z);
return r;
}
Vector2 Vector3::xx() const
{
return Vector2::Create(x,x);
}
Vector2 Vector3::xy() const
{
return Vector2::Create(x,y);
}
Vector2 Vector3::xz() const
{
return Vector2::Create(x,z);
}
Vector2 Vector3::yx() const
{
return Vector2::Create(y,x);
}
Vector2 Vector3::yy() const
{
return Vector2::Create(y,y);
}
Vector2 Vector3::yz() const
{
return Vector2::Create(y,z);
}
Vector2 Vector3::zx() const
{
return Vector2::Create(z,x);
}
Vector2 Vector3::zy() const
{
return Vector2::Create(z,y);
}
Vector2 Vector3::zz() const
{
return Vector2::Create(z,z);
}
void Vector3::xy(const Vector2 &v)
{
x = v.x;
y = v.y;
}
void Vector3::xz(const Vector2 &v)
{
x = v.x;
z = v.y;
}
void Vector3::yx(const Vector2 &v)
{
y = v.x;
x = v.y;
}
void Vector3::yz(const Vector2 &v)
{
y = v.x;
z = v.y;
}
void Vector3::zx(const Vector2 &v)
{
z = v.x;
x = v.y;
}
void Vector3::zy(const Vector2 &v)
{
z = v.x;
y = v.y;
}
Vector3 Vector3::Min() {
return Vector3::Create(-std::numeric_limits<float>::max());
}
Vector3 Vector3::Max() {
return Vector3::Create(std::numeric_limits<float>::max());
}
Vector3 Vector3::Zero() {
return Vector3::Create();
}
Vector3 Vector3::One() {
return Vector3::Create(1.0f, 1.0f, 1.0f);
}
Vector3 Vector3::Forward() {
return Vector3::Create(0.0f, 0.0f, 1.0f);
}
Vector3 Vector3::Backward() {
return Vector3::Create(0.0f, 0.0f, -1.0f);
}
Vector3 Vector3::Up() {
return Vector3::Create(0.0f, 1.0f, 0.0f);
}
Vector3 Vector3::Down() {
return Vector3::Create(0.0f, -1.0f, 0.0f);
}
Vector3 Vector3::Left() {
return Vector3::Create(-1.0f, 0.0f, 0.0f);
}
Vector3 Vector3::Right() {
return Vector3::Create(1.0f, 0.0f, 0.0f);
}
void Vector3::scale(const Vector3 &v)
{
x *= v.x;
y *= v.y;
z *= v.z;
}
Vector3 Vector3::Scale(const Vector3 &v1, const Vector3 &v2)
{
return Vector3::Create(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
}
Vector3 Vector3::Lerp(const Vector3 &v1, const Vector3 &v2, float d) {
return v1 + (v2 - v1) * d;
}
Vector3 Vector3::Slerp(const Vector3 &v1, const Vector3 &v2, float d) {
// From: http://keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/
// Dot product - the cosine of the angle between 2 vectors.
float dot = Vector3::Dot(v1, v2);
// Clamp it to be in the range of Acos()
if(dot < -1.0f) dot = -1.0f;
if(dot > 1.0f) dot = 1.0f;
// Acos(dot) returns the angle between start and end,
// And multiplying that by percent returns the angle between
// start and the final result.
float theta = acos(dot)*d;
Vector3 RelativeVec = v2 - v1*dot;
RelativeVec.normalize(); // Orthonormal basis
// The final result.
return ((v1*cos(theta)) + (RelativeVec*sin(theta)));
}
void Vector3::OrthoNormalize(Vector3 &normal, Vector3 &tangent) {
// Gram-Schmidt Orthonormalization
normal.normalize();
Vector3 proj = normal * Dot(tangent, normal);
tangent = tangent - proj;
tangent.normalize();
}
Vector3& Vector3::operator =(const Vector4 &b) {
x = b.x;
y = b.y;
z = b.z;
return *this;
}
Vector3 Vector3::operator +(const Vector3& b) const {
return Vector3::Create(x + b.x, y + b.y, z + b.z);
}
Vector3 Vector3::operator -(const Vector3& b) const {
return Vector3::Create(x - b.x, y - b.y, z - b.z);
}
Vector3 Vector3::operator +() const {
return *this;
}
Vector3 Vector3::operator -() const {
return Vector3::Create(-x, -y, -z);
}
Vector3 Vector3::operator *(const float v) const {
return Vector3::Create(x * v, y * v, z * v);
}
Vector3 Vector3::operator /(const float v) const {
float inv_v = 1.0f / v;
return Vector3::Create(x * inv_v, y * inv_v, z * inv_v);
}
Vector3& Vector3::operator +=(const Vector3& b) {
x += b.x;
y += b.y;
z += b.z;
return *this;
}
Vector3& Vector3::operator -=(const Vector3& b) {
x -= b.x;
y -= b.y;
z -= b.z;
return *this;
}
Vector3& Vector3::operator *=(const float v) {
x *= v;
y *= v;
z *= v;
return *this;
}
Vector3& Vector3::operator /=(const float v) {
float inv_v = 1.0f / v;
x *= inv_v;
y *= inv_v;
z *= inv_v;
return *this;
}
bool Vector3::operator ==(const Vector3& b) const {
return x == b.x && y == b.y && z == b.z;
}
bool Vector3::operator !=(const Vector3& b) const {
return x != b.x || y != b.y || z != b.z;
}
float& Vector3::operator[](unsigned i) {
switch(i) {
case 0:
return x;
case 1:
return y;
default:
case 2:
return z;
}
}
float Vector3::operator[](unsigned i) const {
switch(i) {
case 0:
return x;
case 1:
return y;
case 2:
default:
return z;
}
}
float Vector3::sqrMagnitude() const {
// calculate the square of the magnitude (useful for comparison of magnitudes without the cost of a sqrt() function)
return x * x + y * y + z * z;
}
float Vector3::magnitude() const {
return sqrtf(x * x + y * y + z * z);
}
void Vector3::normalize() {
float inv_magnitude = 1.0f / sqrtf(x * x + y * y + z * z);
x *= inv_magnitude;
y *= inv_magnitude;
z *= inv_magnitude;
}
Vector3 Vector3::Normalize(const Vector3 &v) {
float inv_magnitude = 1.0f / sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
return Vector3::Create(v.x * inv_magnitude, v.y * inv_magnitude, v.z * inv_magnitude);
}
Vector3 Vector3::Cross(const Vector3 &v1, const Vector3 &v2) {
return Vector3::Create(v1.y * v2.z - v1.z * v2.y,
v1.z * v2.x - v1.x * v2.z,
v1.x * v2.y - v1.y * v2.x);
}
float Vector3::Dot(const Vector3 &v1, const Vector3 &v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
bool Vector3::operator >(const Vector3& b) const
{
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
if(x > b.x) {
return true;
} else if(x < b.x) {
return false;
} else if(y > b.y) {
return true;
} else if(y < b.y) {
return false;
} else if(z > b.z) {
return true;
} else {
return false;
}
}
bool Vector3::operator <(const Vector3& b) const
{
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
if(x < b.x) {
return true;
} else if(x > b.x) {
return false;
} else if(y < b.y) {
return true;
} else if(y > b.y) {
return false;
} else if(z < b.z) {
return true;
} else {
return false;
}
}
} // namespace kraken

335
src/vector4.cpp Normal file
View File

@@ -0,0 +1,335 @@
//
// Vector4.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"
namespace kraken {
//default constructor
void Vector4::init()
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
w = 0.0f;
}
Vector4 Vector4::Create()
{
Vector4 r;
r.init();
return r;
}
void Vector4::init(const Vector4 &v) {
x = v.x;
y = v.y;
z = v.z;
w = v.w;
}
Vector4 Vector4::Create(const Vector4 &v)
{
Vector4 r;
r.init(v);
return r;
}
void Vector4::init(const Vector3 &v, float W) {
x = v.x;
y = v.y;
z = v.z;
w = W;
}
Vector4 Vector4::Create(const Vector3 &v, float W)
{
Vector4 r;
r.init(v, W);
return r;
}
void Vector4::init(float *v) {
x = v[0];
y = v[1];
z = v[2];
w = v[3];
}
Vector4 Vector4::Create(float *v)
{
Vector4 r;
r.init(v);
return r;
}
void Vector4::init(float v) {
x = v;
y = v;
z = v;
w = v;
}
Vector4 Vector4::Create(float v)
{
Vector4 r;
r.init(v);
return r;
}
void Vector4::init(float X, float Y, float Z, float W)
{
x = X;
y = Y;
z = Z;
w = W;
}
Vector4 Vector4::Create(float X, float Y, float Z, float W)
{
Vector4 r;
r.init(X, Y, Z, W);
return r;
}
Vector4 Vector4::Min() {
return Vector4::Create(-std::numeric_limits<float>::max());
}
Vector4 Vector4::Max() {
return Vector4::Create(std::numeric_limits<float>::max());
}
Vector4 Vector4::Zero() {
return Vector4::Create();
}
Vector4 Vector4::One() {
return Vector4::Create(1.0f, 1.0f, 1.0f, 1.0f);
}
Vector4 Vector4::Forward() {
return Vector4::Create(0.0f, 0.0f, 1.0f, 1.0f);
}
Vector4 Vector4::Backward() {
return Vector4::Create(0.0f, 0.0f, -1.0f, 1.0f);
}
Vector4 Vector4::Up() {
return Vector4::Create(0.0f, 1.0f, 0.0f, 1.0f);
}
Vector4 Vector4::Down() {
return Vector4::Create(0.0f, -1.0f, 0.0f, 1.0f);
}
Vector4 Vector4::Left() {
return Vector4::Create(-1.0f, 0.0f, 0.0f, 1.0f);
}
Vector4 Vector4::Right() {
return Vector4::Create(1.0f, 0.0f, 0.0f, 1.0f);
}
Vector4 Vector4::Lerp(const Vector4 &v1, const Vector4 &v2, float d) {
return v1 + (v2 - v1) * d;
}
Vector4 Vector4::Slerp(const Vector4 &v1, const Vector4 &v2, float d) {
// From: http://keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/
// Dot product - the cosine of the angle between 2 vectors.
float dot = Vector4::Dot(v1, v2);
// Clamp it to be in the range of Acos()
if(dot < -1.0f) dot = -1.0f;
if(dot > 1.0f) dot = 1.0f;
// Acos(dot) returns the angle between start and end,
// And multiplying that by percent returns the angle between
// start and the final result.
float theta = acos(dot)*d;
Vector4 RelativeVec = v2 - v1*dot;
RelativeVec.normalize(); // Orthonormal basis
// The final result.
return ((v1*cos(theta)) + (RelativeVec*sin(theta)));
}
void Vector4::OrthoNormalize(Vector4 &normal, Vector4 &tangent) {
// Gram-Schmidt Orthonormalization
normal.normalize();
Vector4 proj = normal * Dot(tangent, normal);
tangent = tangent - proj;
tangent.normalize();
}
Vector4 Vector4::operator +(const Vector4& b) const {
return Vector4::Create(x + b.x, y + b.y, z + b.z, w + b.w);
}
Vector4 Vector4::operator -(const Vector4& b) const {
return Vector4::Create(x - b.x, y - b.y, z - b.z, w - b.w);
}
Vector4 Vector4::operator +() const {
return *this;
}
Vector4 Vector4::operator -() const {
return Vector4::Create(-x, -y, -z, -w);
}
Vector4 Vector4::operator *(const float v) const {
return Vector4::Create(x * v, y * v, z * v, w * v);
}
Vector4 Vector4::operator /(const float v) const {
return Vector4::Create(x / v, y / v, z / v, w/ v);
}
Vector4& Vector4::operator +=(const Vector4& b) {
x += b.x;
y += b.y;
z += b.z;
w += b.w;
return *this;
}
Vector4& Vector4::operator -=(const Vector4& b) {
x -= b.x;
y -= b.y;
z -= b.z;
w -= b.w;
return *this;
}
Vector4& Vector4::operator *=(const float v) {
x *= v;
y *= v;
z *= v;
w *= v;
return *this;
}
Vector4& Vector4::operator /=(const float v) {
float inv_v = 1.0f / v;
x *= inv_v;
y *= inv_v;
z *= inv_v;
w *= inv_v;
return *this;
}
bool Vector4::operator ==(const Vector4& b) const {
return x == b.x && y == b.y && z == b.z && w == b.w;
}
bool Vector4::operator !=(const Vector4& b) const {
return x != b.x || y != b.y || z != b.z || w != b.w;
}
float& Vector4::operator[](unsigned i) {
switch(i) {
case 0:
return x;
case 1:
return y;
case 2:
return z;
default:
case 3:
return w;
}
}
float Vector4::operator[](unsigned i) const {
switch(i) {
case 0:
return x;
case 1:
return y;
case 2:
return z;
default:
case 3:
return w;
}
}
float Vector4::sqrMagnitude() const {
// calculate the square of the magnitude (useful for comparison of magnitudes without the cost of a sqrt() function)
return x * x + y * y + z * z + w * w;
}
float Vector4::magnitude() const {
return sqrtf(x * x + y * y + z * z + w * w);
}
void Vector4::normalize() {
float inv_magnitude = 1.0f / sqrtf(x * x + y * y + z * z + w * w);
x *= inv_magnitude;
y *= inv_magnitude;
z *= inv_magnitude;
w *= inv_magnitude;
}
Vector4 Vector4::Normalize(const Vector4 &v) {
float inv_magnitude = 1.0f / sqrtf(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
return Vector4::Create(v.x * inv_magnitude,
v.y * inv_magnitude,
v.z * inv_magnitude,
v.w * inv_magnitude);
}
float Vector4::Dot(const Vector4 &v1, const Vector4 &v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
}
bool Vector4::operator >(const Vector4& b) const
{
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
if(x != b.x) return x > b.x;
if(y != b.y) return y > b.y;
if(z != b.z) return z > b.z;
if(w != b.w) return w > b.w;
return false;
}
bool Vector4::operator <(const Vector4& b) const
{
// Comparison operators are implemented to allow insertion into sorted containers such as std::set
if(x != b.x) return x < b.x;
if(y != b.y) return y < b.y;
if(z != b.z) return z < b.z;
if(w != b.w) return w < b.w;
return false;
}
} // namespace kraken