first commit
This commit is contained in:
Pascal Serrarens
commit
eaa5702748
391
Quaternion.cpp
Normal file
391
Quaternion.cpp
Normal file
@ -0,0 +1,391 @@
|
||||
//#include "stdafx.h"
|
||||
#include "pch.h"
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
#include "Quaternion.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
void CopyQuat(const Quat& q1, Quat& q2) {
|
||||
q2.x = q1.x;
|
||||
q2.y = q1.y;
|
||||
q2.z = q1.z;
|
||||
q2.w = q1.w;
|
||||
}
|
||||
|
||||
const float Deg2Rad = 0.0174532924F;
|
||||
const float Rad2Deg = 57.29578F;
|
||||
|
||||
Quaternion::Quaternion() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
z = 0;
|
||||
w = 1;
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
w = _w;
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(Vector3 _xyz, float _w) {
|
||||
x = _xyz.x;
|
||||
y = _xyz.y;
|
||||
z = _xyz.z;
|
||||
w = _w;
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(Quat q) {
|
||||
x = q.x;
|
||||
y = q.y;
|
||||
z = q.z;
|
||||
w = q.w;
|
||||
}
|
||||
|
||||
Quaternion::~Quaternion() {}
|
||||
|
||||
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
|
||||
|
||||
Vector3 Quaternion::xyz() const {
|
||||
return Vector3(x, y, z);
|
||||
}
|
||||
|
||||
float Quaternion::GetLength() const {
|
||||
return sqrtf(x * x + y * y + z * z + w * w);
|
||||
}
|
||||
|
||||
float Quaternion::GetLengthSquared() const {
|
||||
return x * x + y * y + z * z + w * w;
|
||||
}
|
||||
float Quaternion::GetLengthSquared(const Quaternion& q) {
|
||||
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
|
||||
}
|
||||
|
||||
void Quaternion::Normalize() {
|
||||
float scale = 1.0f / GetLength();
|
||||
x *= scale;
|
||||
y *= scale;
|
||||
z *= scale;
|
||||
w *= scale;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Normalize(const Quaternion& q) {
|
||||
Quaternion result;
|
||||
Normalize(q, result);
|
||||
return result;
|
||||
};
|
||||
|
||||
void Quaternion::Normalize(const Quaternion& q, Quaternion& result) {
|
||||
float scale = 1.0f / q.GetLength();
|
||||
result = Quaternion(q.x * scale, q.y * scale, q.z * scale, q.w * scale);
|
||||
};
|
||||
|
||||
float Quaternion::Dot(Quaternion a, Quaternion b) {
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
}
|
||||
|
||||
Vector3 Quaternion::ToAngles(const Quaternion& q1) {
|
||||
float test = q1.x * q1.y + q1.z * q1.w;
|
||||
if (test > 0.499) { // singularity at north pole
|
||||
return Vector3(
|
||||
0,
|
||||
2 * (float)atan2(q1.x, q1.w) * Rad2Deg,
|
||||
90
|
||||
);
|
||||
}
|
||||
else if (test < -0.499) { // singularity at south pole
|
||||
return Vector3(
|
||||
0,
|
||||
-2 * (float)atan2(q1.x, q1.w) * Rad2Deg,
|
||||
-90
|
||||
);
|
||||
}
|
||||
else {
|
||||
float sqx = q1.x * q1.x;
|
||||
float sqy = q1.y * q1.y;
|
||||
float sqz = q1.z * q1.z;
|
||||
|
||||
return Vector3(
|
||||
atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) * Rad2Deg,
|
||||
atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) * Rad2Deg,
|
||||
asinf(2 * test) * Rad2Deg
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator *(const Quaternion& r2) const {
|
||||
return Quaternion(
|
||||
this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,
|
||||
-this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y,
|
||||
this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z,
|
||||
-this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w
|
||||
);
|
||||
};
|
||||
|
||||
Vector3 Quaternion::operator *(const Vector3& p) const {
|
||||
float num = this->x * 2;
|
||||
float num2 = this->y * 2;
|
||||
float num3 = this->z * 2;
|
||||
float num4 = this->x * num;
|
||||
float num5 = this->y * num2;
|
||||
float num6 = this->z * num3;
|
||||
float num7 = this->x * num2;
|
||||
float num8 = this->x * num3;
|
||||
float num9 = this->y * num3;
|
||||
float num10 = this->w * num;
|
||||
float num11 = this->w * num2;
|
||||
float num12 = this->w * num3;
|
||||
Vector3 result = Vector3::zero;
|
||||
result.x = (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z;
|
||||
result.y = (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z;
|
||||
result.z = (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z;
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Quaternion::operator==(const Quaternion& q) {
|
||||
return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Inverse(Quaternion r) {
|
||||
float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w);
|
||||
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
|
||||
}
|
||||
|
||||
|
||||
//Quaternion Quaternion::LookRotation(const Vector3 &forward, const Vector3 &upwards) {
|
||||
// return LookRotation(forward, upwards);
|
||||
//}
|
||||
|
||||
|
||||
// This is not correct when forward == Vector3::up
|
||||
Quaternion Quaternion::LookRotation(const Vector3& forward) {
|
||||
Vector3 up = Vector3(0, 1, 0);
|
||||
if ((Vector3)forward == up)
|
||||
up = Vector3(0, 0, 1);
|
||||
return LookRotation(forward, up);
|
||||
}
|
||||
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
|
||||
Vector3 nForward = Vector3::Normalize(forward);
|
||||
Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
|
||||
Vector3 nUp = Vector3::Cross(nForward, nRight);
|
||||
float m00 = nRight.x;
|
||||
float m01 = nRight.y;
|
||||
float m02 = nRight.z;
|
||||
float m10 = nUp.x;
|
||||
float m11 = nUp.y;
|
||||
float m12 = nUp.z;
|
||||
float m20 = nForward.x;
|
||||
float m21 = nForward.y;
|
||||
float m22 = nForward.z;
|
||||
|
||||
|
||||
float num8 = (m00 + m11) + m22;
|
||||
Quaternion quaternion = Quaternion();
|
||||
if (num8 > 0) {
|
||||
float num = sqrtf(num8 + 1);
|
||||
quaternion.w = num * 0.5f;
|
||||
num = 0.5f / num;
|
||||
quaternion.x = (m12 - m21) * num;
|
||||
quaternion.y = (m20 - m02) * num;
|
||||
quaternion.z = (m01 - m10) * num;
|
||||
return quaternion;
|
||||
}
|
||||
if ((m00 >= m11) && (m00 >= m22)) {
|
||||
float num7 = sqrtf(((1 + m00) - m11) - m22);
|
||||
float num4 = 0.5F / num7;
|
||||
quaternion.x = 0.5f * num7;
|
||||
quaternion.y = (m01 + m10) * num4;
|
||||
quaternion.z = (m02 + m20) * num4;
|
||||
quaternion.w = (m12 - m21) * num4;
|
||||
return quaternion;
|
||||
}
|
||||
if (m11 > m22) {
|
||||
float num6 = sqrtf(((1 + m11) - m00) - m22);
|
||||
float num3 = 0.5F / num6;
|
||||
quaternion.x = (m10 + m01) * num3;
|
||||
quaternion.y = 0.5F * num6;
|
||||
quaternion.z = (m21 + m12) * num3;
|
||||
quaternion.w = (m20 - m02) * num3;
|
||||
return quaternion;
|
||||
}
|
||||
float num5 = sqrtf(((1 + m22) - m00) - m11);
|
||||
float num2 = 0.5F / num5;
|
||||
quaternion.x = (m20 + m02) * num2;
|
||||
quaternion.y = (m21 + m12) * num2;
|
||||
quaternion.z = 0.5F * num5;
|
||||
quaternion.w = (m01 - m10) * num2;
|
||||
return quaternion;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) {
|
||||
return RotateTowards(LookRotation(fromDirection), LookRotation(toDirection), FLT_MAX);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta) {
|
||||
float num = Quaternion::Angle(from, to);
|
||||
if (num == 0) {
|
||||
return to;
|
||||
}
|
||||
float t = (float)fmin(1, maxDegreesDelta / num);
|
||||
return SlerpUnclamped(from, to, t);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
|
||||
// return AngleAxis(angle, axis);
|
||||
//}
|
||||
//Quaternion AngleAxis(float angle, Vector3& axis) {
|
||||
if (Vector3::SqrMagnitude(axis) == 0.0)
|
||||
return Quaternion();
|
||||
|
||||
Quaternion result = Quaternion();
|
||||
float radians = angle * Deg2Rad;
|
||||
radians *= 0.5;
|
||||
//Vector3::Normalize(axis);
|
||||
Vector3 axis2 = axis * (float)sin(radians);
|
||||
result.x = axis2.x;
|
||||
result.y = axis2.y;
|
||||
result.z = axis2.z;
|
||||
result.w = (float)cos(radians);
|
||||
|
||||
return Quaternion::Normalize(result);
|
||||
}
|
||||
|
||||
float Quaternion::Angle(Quaternion a, Quaternion b) {
|
||||
float f = Quaternion::Dot(a, b);
|
||||
return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
|
||||
}
|
||||
|
||||
void Quaternion::ToAngleAxis(float* angle, Vector3* axis)
|
||||
{
|
||||
Quaternion::ToAxisAngleRad(*this, axis, angle);
|
||||
*angle *= Rad2Deg;
|
||||
}
|
||||
|
||||
void Quaternion::ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle)
|
||||
{
|
||||
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
|
||||
*angle = 2.0f * acosf(q1.w); // angle
|
||||
float den = sqrtf(1.0F - q1.w * q1.w);
|
||||
if (den > 0.0001f)
|
||||
{
|
||||
*axis = q1.xyz() / den;
|
||||
}
|
||||
else
|
||||
{
|
||||
// This occurs when the angle is zero.
|
||||
// Not a problem: just set an arbitrary normalized axis.
|
||||
*axis = Vector3(1, 0, 0);
|
||||
}
|
||||
}
|
||||
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t) {
|
||||
// if either input is zero, return the other.
|
||||
if (Quaternion::GetLengthSquared(a) == 0.0) {
|
||||
if (Quaternion::GetLengthSquared(b) == 0.0) {
|
||||
return Quaternion();
|
||||
}
|
||||
return b;
|
||||
}
|
||||
else if (Quaternion::GetLengthSquared(b) == 0.0f) {
|
||||
return a;
|
||||
}
|
||||
|
||||
const Vector3 axyz = a.xyz();
|
||||
const Vector3 bxyz = b.xyz();
|
||||
float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
|
||||
|
||||
Quaternion b2 = b;
|
||||
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
|
||||
// angle = 0.0f, so just return one input.
|
||||
return a;
|
||||
}
|
||||
else if (cosHalfAngle < 0.0f) {
|
||||
b2.x = -b.x;
|
||||
b2.y = -b.y;
|
||||
b2.z = -b.z;
|
||||
b2.w = -b.w;
|
||||
cosHalfAngle = -cosHalfAngle;
|
||||
}
|
||||
|
||||
float blendA;
|
||||
float blendB;
|
||||
if (cosHalfAngle < 0.99f) {
|
||||
// do proper slerp for big angles
|
||||
float halfAngle = acosf(cosHalfAngle);
|
||||
float sinHalfAngle = sinf(halfAngle);
|
||||
float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
|
||||
blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
|
||||
blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
|
||||
}
|
||||
else {
|
||||
// do lerp if angle is really small.
|
||||
blendA = 1.0f - t;
|
||||
blendB = t;
|
||||
}
|
||||
|
||||
Quaternion result = Quaternion(axyz * blendA + b2.xyz() * blendB, blendA * a.w + blendB * b2.w);
|
||||
if (result.GetLengthSquared() > 0.0f)
|
||||
return Quaternion::Normalize(result);
|
||||
else
|
||||
return Quaternion();
|
||||
}
|
||||
//Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t) {
|
||||
// return SlerpUnclamped(a, b, t);
|
||||
//}
|
||||
Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) {
|
||||
if (t > 1) t = 1;
|
||||
if (t < 0) t = 0;
|
||||
return Quaternion::SlerpUnclamped(a, b, t);
|
||||
}
|
||||
//Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) {
|
||||
// return Slerp(a, b, t);
|
||||
//}
|
||||
|
||||
Quaternion Quaternion::Euler(float x, float y, float z) {
|
||||
return Quaternion::Euler(Vector3(x, y, z));
|
||||
}
|
||||
Quaternion Quaternion::Euler(Vector3 euler) {
|
||||
return Quaternion::FromEulerRad(euler * Deg2Rad);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::FromEulerRad(Vector3 euler) {
|
||||
float yaw = euler.x;
|
||||
float pitch = euler.y;
|
||||
float roll = euler.z;
|
||||
float rollOver2 = roll * 0.5f;
|
||||
float sinRollOver2 = (float)sin((float)rollOver2);
|
||||
float cosRollOver2 = (float)cos((float)rollOver2);
|
||||
float pitchOver2 = pitch * 0.5f;
|
||||
float sinPitchOver2 = (float)sin((float)pitchOver2);
|
||||
float cosPitchOver2 = (float)cos((float)pitchOver2);
|
||||
float yawOver2 = yaw * 0.5f;
|
||||
float sinYawOver2 = (float)sin((float)yawOver2);
|
||||
float cosYawOver2 = (float)cos((float)yawOver2);
|
||||
Quaternion result;
|
||||
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
||||
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
||||
return result;
|
||||
}
|
||||
|
||||
float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
|
||||
Quaternion secondaryRotation = GetRotationAround(axis, rotation);
|
||||
float rotationAngle;
|
||||
Vector3 rotationAxis;
|
||||
secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
|
||||
|
||||
// Do the axis point in opposite directions?
|
||||
if (Vector3::Dot(axis, rotationAxis) < 0)
|
||||
rotationAngle = -rotationAngle;
|
||||
|
||||
return rotationAngle;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
|
||||
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
|
||||
Vector3 p = Vector3::Project(ra, axis); // return projection ra on to axis (parallel component)
|
||||
Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w);
|
||||
twist = Quaternion::Normalize(twist);
|
||||
return twist;
|
||||
}
|
||||
71
Quaternion.h
Normal file
71
Quaternion.h
Normal file
@ -0,0 +1,71 @@
|
||||
#ifndef QUATERNION_H
|
||||
#define QUATERNION_H
|
||||
|
||||
#pragma once
|
||||
struct Vector3;
|
||||
|
||||
extern "C" {
|
||||
typedef struct Quat {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float w;
|
||||
} Quat;
|
||||
void CopyQuat(const Quat& q1, Quat& q2);
|
||||
}
|
||||
|
||||
|
||||
struct Quaternion : Quat {
|
||||
public:
|
||||
Quaternion();
|
||||
Quaternion(float _x, float _y, float _z, float _w);
|
||||
Quaternion(Vector3 _xyz, float _w);
|
||||
Quaternion(Quat q);
|
||||
~Quaternion();
|
||||
|
||||
const static Quaternion identity;
|
||||
|
||||
float GetLength() const;
|
||||
float GetLengthSquared() const;
|
||||
static float GetLengthSquared(const Quaternion& q);
|
||||
void Normalize();
|
||||
static Quaternion Normalize(const Quaternion& q);
|
||||
static void Normalize(const Quaternion& q, Quaternion& result);
|
||||
|
||||
static float Dot(Quaternion a, Quaternion b);
|
||||
|
||||
static Vector3 ToAngles(const Quaternion& q1);
|
||||
|
||||
Vector3 operator *(const Vector3& p) const;
|
||||
Quaternion operator *(const Quaternion& r2) const;
|
||||
|
||||
bool operator ==(const Quaternion& q);
|
||||
|
||||
static Quaternion Inverse(Quaternion r);
|
||||
|
||||
static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards);
|
||||
static Quaternion LookRotation(const Vector3& forward);
|
||||
|
||||
static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
|
||||
|
||||
static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta);
|
||||
static Quaternion AngleAxis(float angle, const Vector3& axis);
|
||||
void ToAngleAxis(float* angle, Vector3* axis);
|
||||
static void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
|
||||
static float Angle(Quaternion a, Quaternion b);
|
||||
static Quaternion Slerp(const Quaternion& a, const Quaternion& b, float t);
|
||||
static Quaternion SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t);
|
||||
|
||||
static Quaternion Euler(float x, float y, float z);
|
||||
static Quaternion Euler(Vector3 euler);
|
||||
static Quaternion FromEulerRad(Vector3 euler);
|
||||
static float GetAngleAround(Vector3 axis, Quaternion rotation);
|
||||
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
|
||||
public:
|
||||
Vector3 xyz() const;
|
||||
};
|
||||
|
||||
//#include "Vector3.h"
|
||||
|
||||
|
||||
#endif
|
||||
205
Vector3.cpp
Normal file
205
Vector3.cpp
Normal file
@ -0,0 +1,205 @@
|
||||
//#include "stdafx.h"
|
||||
#include "pch.h"
|
||||
#include <math.h>
|
||||
#include "Vector3.h"
|
||||
#include "Quaternion.h"
|
||||
|
||||
const float Deg2Rad = 0.0174532924F;
|
||||
const float Rad2Deg = 57.29578F;
|
||||
|
||||
Vector3::Vector3() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
z = 0;
|
||||
}
|
||||
|
||||
Vector3::Vector3(float _x, float _y, float _z) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
}
|
||||
|
||||
Vector3::Vector3(Vec3 v) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
}
|
||||
|
||||
Vector3::~Vector3()
|
||||
{
|
||||
}
|
||||
|
||||
const Vector3 Vector3::zero = Vector3(0, 0, 0);
|
||||
const Vector3 Vector3::right = Vector3(1, 0, 0);
|
||||
const Vector3 Vector3::left = Vector3(-1, 0, 0);
|
||||
const Vector3 Vector3::up = Vector3(0, 1, 0);
|
||||
const Vector3 Vector3::down = Vector3(0, -1, 0);
|
||||
const Vector3 Vector3::forward = Vector3(0, 0, 1);
|
||||
const Vector3 Vector3::back = Vector3(0, 0, -1);
|
||||
|
||||
float Vector3::Magnitude(const Vector3& a) {
|
||||
return sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
|
||||
}
|
||||
float Vector3::magnitude() const {
|
||||
return (float)sqrtf(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
float Vector3::SqrMagnitude(const Vector3& a) {
|
||||
return a.x * a.x + a.y * a.y + a.z * a.z;
|
||||
}
|
||||
float Vector3::sqrMagnitude() const {
|
||||
return(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
Vector3 Vector3::Normalize(Vector3 v) {
|
||||
float num = Vector3::Magnitude(v);
|
||||
Vector3 result = Vector3::zero;
|
||||
if (num > 1E-05f) {
|
||||
result = v / num;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
Vector3 Vector3::normalized() const {
|
||||
float num = this->magnitude();
|
||||
Vector3 result = Vector3::zero;
|
||||
if (num > 1E-05f) {
|
||||
result = ((Vector3)*this) / num;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator -(const Vector3& v2) const {
|
||||
return Vector3(this->x - v2.x, this->y - v2.y, this->z - v2.z);
|
||||
}
|
||||
//Vector operator -(const Vector& v1, const Vector& v2) {
|
||||
// return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
|
||||
//}
|
||||
//Vector Vector::operator -(const Vector& v1, const Vector& v2) {
|
||||
// return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
|
||||
//}
|
||||
Vector3 Vector3::operator -() {
|
||||
return Vector3(-this->x, -this->y, -this->z);
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator +(const Vector3& v2) const {
|
||||
return Vector3(this->x + v2.x, this->y + v2.y, this->z + v2.z);
|
||||
}
|
||||
Vector3 Vector3::Scale(const Vector3& p1, const Vector3& p2) {
|
||||
return Vector3(p1.x * p2.x, p1.y * p2.y, p1.z * p2.z);
|
||||
}
|
||||
|
||||
//Vector Vector::operator *(float f, const Vector& p) {
|
||||
// return Vector(f * p.x, f * p.y, f * p.z);
|
||||
//}
|
||||
Vector3 Vector3::operator *(float f) const {
|
||||
return Vector3(this->x * f, this->y * f, this->z * f);
|
||||
}
|
||||
//Vector Vector::operator*(const Vector& v, float f) {
|
||||
// return Vector(v.x * f, v.y * f, v.z * f);
|
||||
//}
|
||||
//Vector Vector::operator*(float f, const Vector& v) {
|
||||
// return Vector(f * v.x, f * v.y, f * v.z);
|
||||
//}
|
||||
|
||||
Vector3 operator *(const Quaternion& o, const Vector3& p) {
|
||||
float num = o.x * 2;
|
||||
float num2 = o.y * 2;
|
||||
float num3 = o.z * 2;
|
||||
float num4 = o.x * num;
|
||||
float num5 = o.y * num2;
|
||||
float num6 = o.z * num3;
|
||||
float num7 = o.x * num2;
|
||||
float num8 = o.x * num3;
|
||||
float num9 = o.y * num3;
|
||||
float num10 = o.w * num;
|
||||
float num11 = o.w * num2;
|
||||
float num12 = o.w * num3;
|
||||
Vector3 result = Vector3::zero;
|
||||
result.x = (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z;
|
||||
result.y = (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z;
|
||||
result.z = (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z;
|
||||
return result;
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator/(const float& d) {
|
||||
return Vector3(this->x / d, this->y / d, this->z / d);
|
||||
}
|
||||
|
||||
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& v) {
|
||||
return (this->x == v.x && this->y == v.y && this->z == v.z);
|
||||
}
|
||||
|
||||
float Vector3::Distance(const Vector3& p1, const Vector3& p2) {
|
||||
return Magnitude(p1 - p2);
|
||||
}
|
||||
|
||||
Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) {
|
||||
return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
|
||||
}
|
||||
|
||||
// Projects a vector onto another vector.
|
||||
Vector3 Vector3::Project(Vector3 vector, Vector3 onNormal) {
|
||||
float sqrMag = Dot(onNormal, onNormal);
|
||||
if (sqrMag < 1E-05f)
|
||||
return Vector3::zero;
|
||||
else
|
||||
return onNormal * Dot(vector, onNormal) / sqrMag;
|
||||
}
|
||||
|
||||
// Projects a vector onto a plane defined by a normal orthogonal to the plane.
|
||||
Vector3 Vector3::ProjectOnPlane(Vector3 vector, Vector3 planeNormal) {
|
||||
return vector - Project(vector, planeNormal);
|
||||
}
|
||||
|
||||
float clamp(float x, float upper, float lower) {
|
||||
return fminf(upper, fmaxf(x, lower));
|
||||
}
|
||||
|
||||
// This function is buggy (0-1-0, 0-1-0) gives 180 degrees in return!!!
|
||||
float Vector3::Angle(Vector3 from, Vector3 to) {
|
||||
// sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
|
||||
float denominator = sqrtf(from.sqrMagnitude() * to.sqrMagnitude());
|
||||
if (denominator < 1E-05f)
|
||||
return 0;
|
||||
|
||||
float dot = clamp(Vector3::Dot(from, to) / denominator, -1.0, 1.0);
|
||||
return ((float)acos(dot)) * Rad2Deg;
|
||||
}
|
||||
|
||||
float Vector3::SignedAngle(Vector3 from, Vector3 to, Vector3 axis) {
|
||||
// angle in [0,180]
|
||||
float angle = Vector3::Angle(from, to);
|
||||
float signd = signbit(Vector3::Dot(axis, Vector3::Cross(from, to)));
|
||||
|
||||
// angle in [-179,180]
|
||||
float signed_angle = angle * signd;
|
||||
|
||||
// angle in [0,360] (not used but included here for completeness)
|
||||
//float angle360 = (signed_angle + 180) % 360;
|
||||
|
||||
return signed_angle;
|
||||
}
|
||||
|
||||
|
||||
void CopyVec3(const Vec3& v1, Vec3& v2) {
|
||||
v2.x = v1.x;
|
||||
v2.y = v1.y;
|
||||
v2.z = v1.z;
|
||||
}
|
||||
|
||||
void Vec3_Constructor(Vec3* v, float _x, float _y, float _z) {
|
||||
v->x = _x;
|
||||
v->y = _y;
|
||||
v->z = _z;
|
||||
}
|
||||
|
||||
//Vec3 new_Vec3(float _x, float _y, float _z) {
|
||||
// Vec3 v = Vec3();
|
||||
// v.x = _x;
|
||||
// return v;
|
||||
//}
|
||||
71
Vector3.h
Normal file
71
Vector3.h
Normal file
@ -0,0 +1,71 @@
|
||||
#ifndef VECTOR_H
|
||||
#define VECTOR_H
|
||||
|
||||
#pragma once
|
||||
struct Quaternion;
|
||||
|
||||
extern "C" {
|
||||
typedef struct Vec3 {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
} Vec3;
|
||||
|
||||
void CopyVec3(const Vec3& v1, Vec3& v2);
|
||||
}
|
||||
|
||||
struct Vector3 : Vec3 {
|
||||
public:
|
||||
Vector3();
|
||||
Vector3(float _x, float _y, float _z);
|
||||
Vector3(Vec3 v);
|
||||
~Vector3();
|
||||
|
||||
const static Vector3 zero;
|
||||
const static Vector3 right;
|
||||
const static Vector3 left;
|
||||
const static Vector3 up;
|
||||
const static Vector3 down;
|
||||
const static Vector3 forward;
|
||||
const static Vector3 back;
|
||||
|
||||
static float Magnitude(const Vector3& a);
|
||||
float magnitude() const;
|
||||
static float SqrMagnitude(const Vector3& a);
|
||||
float sqrMagnitude() const;
|
||||
static Vector3 Normalize(Vector3 v);
|
||||
Vector3 normalized() const;
|
||||
|
||||
Vector3 operator -();
|
||||
Vector3 operator -(const Vector3& p2) const;
|
||||
|
||||
Vector3 operator +(const Vector3& t1) const;
|
||||
static Vector3 Scale(const Vector3& p1, const Vector3& p2);
|
||||
|
||||
Vector3 operator *(float f) const;
|
||||
|
||||
Vector3 operator /(const float& d);
|
||||
static float Dot(const Vector3& v1, const Vector3& v2);
|
||||
|
||||
bool operator ==(const Vector3& v);
|
||||
|
||||
static float Distance(const Vector3& p1, const Vector3& p2);
|
||||
static Vector3 Cross(const Vector3& v1, const Vector3& v2);
|
||||
|
||||
// Projects a vector onto another vector.
|
||||
static Vector3 Project(Vector3 vector, Vector3 onNormal);
|
||||
// Projects a vector onto a plane defined by a normal orthogonal to the plane.
|
||||
static Vector3 ProjectOnPlane(Vector3 vector, Vector3 planeNormal);
|
||||
|
||||
static float Angle(Vector3 from, Vector3 to);
|
||||
static float SignedAngle(Vector3 from, Vector3 to, Vector3 axis);
|
||||
};
|
||||
|
||||
void Vec3_Constructor(Vec3* v, float _x, float _y, float _z);
|
||||
Vec3 new_Vec3(float _x, float _y, float _z);
|
||||
|
||||
|
||||
//#include "Quaternion.h"
|
||||
|
||||
#endif
|
||||
Loading…
x
Reference in New Issue
Block a user