mirror of
https://github.com/leozide/leocad
synced 2025-01-17 18:11:42 +01:00
Deleted old algebra files.
This commit is contained in:
leo
parent
ce65a22250
commit
ad5351cbbf
8 changed files with 414 additions and 1405 deletions
|
|
@ -1,511 +0,0 @@
|
|||
//
|
||||
// Math and Linear Algebra stuff.
|
||||
//
|
||||
|
||||
#include "lc_global.h"
|
||||
#include <float.h>
|
||||
#include "algebra.h"
|
||||
#include "lc_math.h"
|
||||
|
||||
// ============================================================================
|
||||
// 4x4 Matrix class.
|
||||
|
||||
void Matrix44::CreateLookAt(const Vector3& Eye, const Vector3& Target, const Vector3& Up)
|
||||
{
|
||||
Vector3 x, y, z;
|
||||
|
||||
// Z = Eye - Target
|
||||
z = Eye - Target;
|
||||
|
||||
// X = Y Cross Z
|
||||
x = Cross3(Up, z);
|
||||
|
||||
// Y = Z Cross X
|
||||
y = Cross3(z, x);
|
||||
|
||||
// Normalize everything.
|
||||
x.Normalize();
|
||||
y.Normalize();
|
||||
z.Normalize();
|
||||
|
||||
m_Rows[0] = Vector4(x[0], y[0], z[0], 0.0f);
|
||||
m_Rows[1] = Vector4(x[1], y[1], z[1], 0.0f);
|
||||
m_Rows[2] = Vector4(x[2], y[2], z[2], 0.0f);
|
||||
m_Rows[3] = m_Rows[0]*-Eye[0] + m_Rows[1]*-Eye[1] + m_Rows[2]*-Eye[2];
|
||||
m_Rows[3][3] = 1.0f;
|
||||
}
|
||||
|
||||
void Matrix44::CreatePerspective(float FoVy, float Aspect, float Near, float Far)
|
||||
{
|
||||
float Left, Right, Bottom, Top;
|
||||
|
||||
Top = Near * (float)tan(FoVy * LC_PI / 360.0f);
|
||||
Bottom = -Top;
|
||||
|
||||
Left = Bottom * Aspect;
|
||||
Right = Top * Aspect;
|
||||
|
||||
if ((Near <= 0.0f) || (Far <= 0.0f) || (Near == Far) || (Left == Right) || (Top == Bottom))
|
||||
return;
|
||||
|
||||
float x, y, a, b, c, d;
|
||||
|
||||
x = (2.0f * Near) / (Right - Left);
|
||||
y = (2.0f * Near) / (Top - Bottom);
|
||||
a = (Right + Left) / (Right - Left);
|
||||
b = (Top + Bottom) / (Top - Bottom);
|
||||
c = -(Far + Near) / (Far - Near);
|
||||
d = -(2.0f * Far * Near) / (Far - Near);
|
||||
|
||||
m_Rows[0] = Vector4(x, 0, 0, 0);
|
||||
m_Rows[1] = Vector4(0, y, 0, 0);
|
||||
m_Rows[2] = Vector4(a, b, c, -1);
|
||||
m_Rows[3] = Vector4(0, 0, d, 0);
|
||||
}
|
||||
|
||||
void Matrix44::CreateOrtho(float Left, float Right, float Bottom, float Top, float Near, float Far)
|
||||
{
|
||||
m_Rows[0] = Vector4(2.0f / (Right-Left), 0.0f, 0.0f, 0.0f);
|
||||
m_Rows[1] = Vector4(0.0f, 2.0f / (Top-Bottom), 0.0f, 0.0f);
|
||||
m_Rows[2] = Vector4(0.0f, 0.0f, -2.0f / (Far-Near), 0.0f);
|
||||
m_Rows[3] = Vector4(-(Right+Left) / (Right-Left), -(Top+Bottom) / (Top-Bottom), -(Far+Near) / (Far-Near), 1.0f);
|
||||
}
|
||||
|
||||
void GetFrustumPlanes(const Matrix44& WorldView, const Matrix44& Projection, Vector4 Planes[6])
|
||||
{
|
||||
// TODO: Use vectors.
|
||||
Matrix44 WorldProj = Mul(WorldView, Projection);
|
||||
|
||||
Planes[0][0] = (WorldProj[0][0] - WorldProj[0][3]) * -1;
|
||||
Planes[0][1] = (WorldProj[1][0] - WorldProj[1][3]) * -1;
|
||||
Planes[0][2] = (WorldProj[2][0] - WorldProj[2][3]) * -1;
|
||||
Planes[0][3] = (WorldProj[3][0] - WorldProj[3][3]) * -1;
|
||||
Planes[1][0] = WorldProj[0][0] + WorldProj[0][3];
|
||||
Planes[1][1] = WorldProj[1][0] + WorldProj[1][3];
|
||||
Planes[1][2] = WorldProj[2][0] + WorldProj[2][3];
|
||||
Planes[1][3] = WorldProj[3][0] + WorldProj[3][3];
|
||||
Planes[2][0] = (WorldProj[0][1] - WorldProj[0][3]) * -1;
|
||||
Planes[2][1] = (WorldProj[1][1] - WorldProj[1][3]) * -1;
|
||||
Planes[2][2] = (WorldProj[2][1] - WorldProj[2][3]) * -1;
|
||||
Planes[2][3] = (WorldProj[3][1] - WorldProj[3][3]) * -1;
|
||||
Planes[3][0] = WorldProj[0][1] + WorldProj[0][3];
|
||||
Planes[3][1] = WorldProj[1][1] + WorldProj[1][3];
|
||||
Planes[3][2] = WorldProj[2][1] + WorldProj[2][3];
|
||||
Planes[3][3] = WorldProj[3][1] + WorldProj[3][3];
|
||||
Planes[4][0] = (WorldProj[0][2] - WorldProj[0][3]) * -1;
|
||||
Planes[4][1] = (WorldProj[1][2] - WorldProj[1][3]) * -1;
|
||||
Planes[4][2] = (WorldProj[2][2] - WorldProj[2][3]) * -1;
|
||||
Planes[4][3] = (WorldProj[3][2] - WorldProj[3][3]) * -1;
|
||||
Planes[5][0] = WorldProj[0][2] + WorldProj[0][3];
|
||||
Planes[5][1] = WorldProj[1][2] + WorldProj[1][3];
|
||||
Planes[5][2] = WorldProj[2][2] + WorldProj[2][3];
|
||||
Planes[5][3] = WorldProj[3][2] + WorldProj[3][3];
|
||||
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
float Len = Vector3(Planes[i]).Length();
|
||||
Planes[i] /= -Len;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 ZoomExtents(const Vector3& Position, const Matrix44& WorldView, const Matrix44& Projection, const Vector3* Points, int NumPoints)
|
||||
{
|
||||
if (!NumPoints)
|
||||
return Position;
|
||||
|
||||
Vector4 Planes[6];
|
||||
GetFrustumPlanes(WorldView, Projection, Planes);
|
||||
|
||||
Vector3 Front = Vector3(WorldView[0][2], WorldView[1][2], WorldView[2][2]);
|
||||
|
||||
// Calculate the position that is as close as possible to the model and has all pieces visible.
|
||||
float SmallestDistance = FLT_MAX;
|
||||
|
||||
for (int p = 0; p < 4; p++)
|
||||
{
|
||||
float ep = Dot3(Position, Planes[p]);
|
||||
float fp = Dot3(Front, Planes[p]);
|
||||
|
||||
for (int j = 0; j < NumPoints; j++)
|
||||
{
|
||||
// Intersect the camera line with the plane that contains this point, NewEye = Eye + u * (Target - Eye)
|
||||
float u = (ep - Dot3(Points[j], Planes[p])) / fp;
|
||||
|
||||
if (u < SmallestDistance)
|
||||
SmallestDistance = u;
|
||||
}
|
||||
}
|
||||
|
||||
return Position - (Front * SmallestDistance);
|
||||
}
|
||||
|
||||
// Inverse code from the GLU library.
|
||||
Matrix44 Inverse(const Matrix44& m)
|
||||
{
|
||||
#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
|
||||
#define MAT(m,c,r) m.m_Rows[r][c]
|
||||
|
||||
float wtmp[4][8];
|
||||
float m0, m1, m2, m3, s;
|
||||
float *r0, *r1, *r2, *r3;
|
||||
|
||||
r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
|
||||
|
||||
r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
|
||||
r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
|
||||
r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
|
||||
|
||||
r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
|
||||
r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
|
||||
r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
|
||||
|
||||
r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
|
||||
r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
|
||||
r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
|
||||
|
||||
r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
|
||||
r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
|
||||
r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
|
||||
|
||||
// choose pivot - or die
|
||||
if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
|
||||
if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
|
||||
if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
|
||||
// if (0.0 == r0[0]) return GL_FALSE;
|
||||
|
||||
// eliminate first variable
|
||||
m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
|
||||
s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
|
||||
s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
|
||||
s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
|
||||
s = r0[4];
|
||||
if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
|
||||
s = r0[5];
|
||||
if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
|
||||
s = r0[6];
|
||||
if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
|
||||
s = r0[7];
|
||||
if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
|
||||
|
||||
// choose pivot - or die
|
||||
if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
|
||||
if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
|
||||
// if (0.0 == r1[1]) return GL_FALSE;
|
||||
|
||||
// eliminate second variable
|
||||
m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
|
||||
r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
|
||||
r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
|
||||
s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
|
||||
s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
|
||||
s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
|
||||
s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
|
||||
|
||||
// choose pivot - or die
|
||||
if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
|
||||
// if (0.0 == r2[2]) return GL_FALSE;
|
||||
|
||||
// eliminate third variable
|
||||
m3 = r3[2]/r2[2];
|
||||
r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
|
||||
r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
|
||||
r3[7] -= m3 * r2[7];
|
||||
|
||||
// last check
|
||||
// if (0.0 == r3[3]) return GL_FALSE;
|
||||
|
||||
s = 1.0f/r3[3]; // now back substitute row 3
|
||||
r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
|
||||
|
||||
m2 = r2[3]; // now back substitute row 2
|
||||
s = 1.0f/r2[2];
|
||||
r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
|
||||
r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
|
||||
m1 = r1[3];
|
||||
r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
|
||||
r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
|
||||
m0 = r0[3];
|
||||
r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
|
||||
r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
|
||||
|
||||
m1 = r1[2]; // now back substitute row 1
|
||||
s = 1.0f/r1[1];
|
||||
r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
|
||||
r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
|
||||
m0 = r0[2];
|
||||
r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
|
||||
r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
|
||||
|
||||
m0 = r0[1]; // now back substitute row 0
|
||||
s = 1.0f/r0[0];
|
||||
r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
|
||||
r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
|
||||
|
||||
Vector4 Row0(r0[4], r1[4], r2[4], r3[4]);
|
||||
Vector4 Row1(r0[5], r1[5], r2[5], r3[5]);
|
||||
Vector4 Row2(r0[6], r1[6], r2[6], r3[6]);
|
||||
Vector4 Row3(r0[7], r1[7], r2[7], r3[7]);
|
||||
|
||||
Matrix44 out(Row0, Row1, Row2, Row3);
|
||||
|
||||
return out;
|
||||
|
||||
#undef MAT
|
||||
#undef SWAP_ROWS
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Project/Unproject a point.
|
||||
|
||||
// Convert world coordinates to screen coordinates.
|
||||
Vector3 ProjectPoint(const Vector3& Pt, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
|
||||
{
|
||||
Vector4 Tmp;
|
||||
|
||||
Tmp = Mul4(Vector4(Pt[0], Pt[1], Pt[2], 1.0f), ModelView);
|
||||
Tmp = Mul4(Tmp, Projection);
|
||||
|
||||
// Normalize.
|
||||
Tmp /= Tmp[3];
|
||||
|
||||
// Screen coordinates.
|
||||
return Vector3(Viewport[0]+(1+Tmp[0])*Viewport[2]/2, Viewport[1]+(1+Tmp[1])*Viewport[3]/2, (1+Tmp[2])/2);
|
||||
}
|
||||
|
||||
void ProjectPoints(Vector3* Points, int NumPoints, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
|
||||
{
|
||||
for (int i = 0; i < NumPoints; i++)
|
||||
{
|
||||
Vector4 Tmp;
|
||||
|
||||
Tmp = Mul4(Vector4(Points[i][0], Points[i][1], Points[i][2], 1.0f), ModelView);
|
||||
Tmp = Mul4(Tmp, Projection);
|
||||
|
||||
// Normalize.
|
||||
Tmp /= Tmp[3];
|
||||
|
||||
// Screen coordinates.
|
||||
Points[i] = Vector3(Viewport[0]+(1+Tmp[0])*Viewport[2]/2, Viewport[1]+(1+Tmp[1])*Viewport[3]/2, (1+Tmp[2])/2);
|
||||
}
|
||||
}
|
||||
|
||||
// Convert screen coordinates to world coordinates.
|
||||
Vector3 UnprojectPoint(const Vector3& Point, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
|
||||
{
|
||||
Vector3 Tmp = Point;
|
||||
UnprojectPoints(&Tmp, 1, ModelView, Projection, Viewport);
|
||||
return Tmp;
|
||||
}
|
||||
|
||||
void UnprojectPoints(Vector3* Points, int NumPoints, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
|
||||
{
|
||||
// Calculate the screen to model transform.
|
||||
Matrix44 Transform = Inverse(Mul(ModelView, Projection));
|
||||
|
||||
for (int i = 0; i < NumPoints; i++)
|
||||
{
|
||||
Vector4 Tmp;
|
||||
|
||||
// Convert the point to homogeneous coordinates.
|
||||
Tmp[0] = (Points[i][0] - Viewport[0]) * 2.0f / Viewport[2] - 1.0f;
|
||||
Tmp[1] = (Points[i][1] - Viewport[1]) * 2.0f / Viewport[3] - 1.0f;
|
||||
Tmp[2] = Points[i][2] * 2.0f - 1.0f;
|
||||
Tmp[3] = 1.0f;
|
||||
|
||||
Tmp = Mul4(Tmp, Transform);
|
||||
|
||||
if (Tmp[3] != 0.0f)
|
||||
Tmp /= Tmp[3];
|
||||
|
||||
Points[i] = Vector3(Tmp[0], Tmp[1], Tmp[2]);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Geometry functions.
|
||||
|
||||
// Sutherland-Hodgman method of clipping a polygon to a plane.
|
||||
void PolygonPlaneClip(Vector3* InPoints, int NumInPoints, Vector3* OutPoints, int* NumOutPoints, const Vector4& Plane)
|
||||
{
|
||||
Vector3 *s, *p, i;
|
||||
|
||||
*NumOutPoints = 0;
|
||||
s = &InPoints[NumInPoints-1];
|
||||
|
||||
for (int j = 0; j < NumInPoints; j++)
|
||||
{
|
||||
p = &InPoints[j];
|
||||
|
||||
if (Dot3(*p, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
if (Dot3(*s, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
// Both points inside.
|
||||
OutPoints[*NumOutPoints] = *p;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Outside, inside.
|
||||
LinePlaneIntersection(i, *s, *p, Plane);
|
||||
|
||||
OutPoints[*NumOutPoints] = i;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
OutPoints[*NumOutPoints] = *p;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Dot3(*s, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
// Inside, outside.
|
||||
LinePlaneIntersection(i, *s, *p, Plane);
|
||||
|
||||
OutPoints[*NumOutPoints] = i;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
}
|
||||
|
||||
s = p;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate the intersection of a line segment and a plane and returns false
|
||||
// if they are parallel or the intersection is outside the line segment.
|
||||
bool LinePlaneIntersection(Vector3& Intersection, const Vector3& Start, const Vector3& End, const Vector4& Plane)
|
||||
{
|
||||
Vector3 Dir = End - Start;
|
||||
|
||||
float t1 = Dot3(Plane, Start) + Plane[3];
|
||||
float t2 = Dot3(Plane, Dir);
|
||||
|
||||
if (t2 == 0.0f)
|
||||
return false;
|
||||
|
||||
float t = -t1 / t2;
|
||||
|
||||
Intersection = Start + t * Dir;
|
||||
|
||||
if ((t < 0.0f) || (t > 1.0f))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool LineTriangleMinIntersection(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& Start, const Vector3& End, float& MinDist, Vector3& Intersection)
|
||||
{
|
||||
// Calculate the polygon plane.
|
||||
Vector4 Plane;
|
||||
Plane = Cross3(p1 - p2, p3 - p2);
|
||||
Plane[3] = -Dot3(Plane, p1);
|
||||
|
||||
// Check if the line is parallel to the plane.
|
||||
Vector3 Dir = End - Start;
|
||||
|
||||
float t1 = Dot3(Plane, Start) + Plane[3];
|
||||
float t2 = Dot3(Plane, Dir);
|
||||
|
||||
if (t2 == 0)
|
||||
return false;
|
||||
|
||||
float t = -(t1 / t2);
|
||||
|
||||
if (t < 0)
|
||||
return false;
|
||||
|
||||
// Intersection of the plane and line segment.
|
||||
Intersection = Start - (t1 / t2) * Dir;
|
||||
|
||||
float Dist = (Start - Intersection).Length();
|
||||
|
||||
if (Dist > MinDist)
|
||||
return false;
|
||||
|
||||
// Check if we're inside the triangle.
|
||||
Vector3 pa1, pa2, pa3;
|
||||
pa1 = (p1 - Intersection).Normalize();
|
||||
pa2 = (p2 - Intersection).Normalize();
|
||||
pa3 = (p3 - Intersection).Normalize();
|
||||
|
||||
float a1, a2, a3;
|
||||
a1 = Dot3(pa1, pa2);
|
||||
a2 = Dot3(pa2, pa3);
|
||||
a3 = Dot3(pa3, pa1);
|
||||
|
||||
float total = (acosf(a1) + acosf(a2) + acosf(a3)) * LC_RTOD;
|
||||
|
||||
if (fabs(total - 360) <= 0.001f)
|
||||
{
|
||||
MinDist = Dist;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool LineQuadMinIntersection(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, const Vector3& Start, const Vector3& End, float& MinDist, Vector3& Intersection)
|
||||
{
|
||||
// Calculate the polygon plane.
|
||||
Vector4 Plane;
|
||||
Plane = Cross3(p1 - p2, p3 - p2);
|
||||
Plane[3] = -Dot3(Plane, p1);
|
||||
|
||||
// Check if the line is parallel to the plane.
|
||||
Vector3 Dir = End - Start;
|
||||
|
||||
float t1 = Dot3(Plane, Start) + Plane[3];
|
||||
float t2 = Dot3(Plane, Dir);
|
||||
|
||||
if (t2 == 0)
|
||||
return false;
|
||||
|
||||
float t = -(t1 / t2);
|
||||
|
||||
if (t < 0)
|
||||
return false;
|
||||
|
||||
// Intersection of the plane and line segment.
|
||||
Intersection = Start - (t1 / t2) * Dir;
|
||||
|
||||
float Dist = (Start - Intersection).Length();
|
||||
|
||||
if (Dist > MinDist)
|
||||
return false;
|
||||
|
||||
// Check if we're inside the triangle.
|
||||
Vector3 pa1, pa2, pa3;
|
||||
pa1 = (p1 - Intersection).Normalize();
|
||||
pa2 = (p2 - Intersection).Normalize();
|
||||
pa3 = (p3 - Intersection).Normalize();
|
||||
|
||||
float a1, a2, a3;
|
||||
a1 = Dot3(pa1, pa2);
|
||||
a2 = Dot3(pa2, pa3);
|
||||
a3 = Dot3(pa3, pa1);
|
||||
|
||||
float total = (acosf(a1) + acosf(a2) + acosf(a3)) * LC_RTOD;
|
||||
|
||||
if (fabs(total - 360) <= 0.001f)
|
||||
{
|
||||
MinDist = Dist;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check if we're inside the second triangle.
|
||||
pa2 = (p4 - Intersection).Normalize();
|
||||
|
||||
a1 = Dot3(pa1, pa2);
|
||||
a2 = Dot3(pa2, pa3);
|
||||
a3 = Dot3(pa3, pa1);
|
||||
|
||||
total = (acosf(a1) + acosf(a2) + acosf(a3)) * LC_RTOD;
|
||||
|
||||
if (fabs(total - 360) <= 0.001f)
|
||||
{
|
||||
MinDist = Dist;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
713
common/algebra.h
713
common/algebra.h
|
|
@ -1,713 +0,0 @@
|
|||
#ifndef _ALGEBRA_H_
|
||||
#define _ALGEBRA_H_
|
||||
|
||||
#include <math.h>
|
||||
|
||||
//
|
||||
// Simple math library and linear algebra functions.
|
||||
//
|
||||
// Everything is based on the Vector4 class, so changing that class should be enough
|
||||
// to add support for compiler specific math intrinsics.
|
||||
//
|
||||
// Functions that end with 34 mean that they don't care what happens to the 4th
|
||||
// component, it can either be affected or not.
|
||||
//
|
||||
// Matrices are represented as row-major, so we pre-multiply instead of post-multiplying
|
||||
// like you would in a column major notation.
|
||||
//
|
||||
// OpenGL only expects a matrix to be an array of 16 floats so it doesn't matter what
|
||||
// notation we use.
|
||||
//
|
||||
// v[0] v[1] v[2] v[3] <- x, y, z, w
|
||||
//
|
||||
// m[0] m[1] m[2] m[3] <- x axis
|
||||
// m[4] m[5] m[6] m[7] <- y axis
|
||||
// m[8] m[9] m[10] m[11] <- z axis
|
||||
// m[12] m[13] m[14] m[15] <- translation
|
||||
//
|
||||
|
||||
// TODO: Move this define to config.h
|
||||
#define LC_MATH_FLOAT
|
||||
//#define LC_MATH_SSE
|
||||
|
||||
// Classes defined in this file:
|
||||
class Vector3;
|
||||
class Vector4;
|
||||
class Quaternion;
|
||||
class Matrix44;
|
||||
|
||||
// ============================================================================
|
||||
// Vector4 class (float version).
|
||||
|
||||
#ifdef LC_MATH_FLOAT
|
||||
|
||||
class Vector4
|
||||
{
|
||||
public:
|
||||
// Constructors.
|
||||
inline Vector4() { }
|
||||
inline explicit Vector4(const float _x, const float _y, const float _z)
|
||||
: x(_x), y(_y), z(_z) { }
|
||||
inline explicit Vector4(const float _x, const float _y, const float _z, const float _w)
|
||||
: x(_x), y(_y), z(_z), w(_w) { }
|
||||
|
||||
inline operator const float*() const { return (const float*)this; }
|
||||
inline float& operator[](int i) const { return ((float*)this)[i]; }
|
||||
|
||||
// Comparison.
|
||||
friend inline bool operator==(const Vector4& a, const Vector4& b)
|
||||
{ return (a.x == b.x) && (a.y == b.y) && (a.z == b.z) && (a.w == b.w); }
|
||||
|
||||
friend inline bool Compare3(const Vector4& a, const Vector4& b)
|
||||
{ return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); }
|
||||
|
||||
// Math operations for 4 components.
|
||||
friend inline Vector4 operator+(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w); }
|
||||
|
||||
friend inline Vector4 operator-(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.x-b.x, a.y-b.y, a.z-b.z, a.w-b.w); }
|
||||
|
||||
friend inline Vector4 operator*(const Vector4& a, float f)
|
||||
{ return Vector4(a.x*f, a.y*f, a.z*f, a.w*f); }
|
||||
|
||||
friend inline Vector4 operator*(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.x*b.x, a.y*b.y, a.z*b.z, a.w*b.w); }
|
||||
|
||||
friend inline Vector4 operator/(const Vector4& a, float f)
|
||||
{ return Vector4(a.x/f, a.y/f, a.z/f, a.w/f); }
|
||||
|
||||
friend inline Vector4 operator/=(Vector4& a, float f)
|
||||
{ a = Vector4(a.x/f, a.y/f, a.z/f, a.w/f); return a; }
|
||||
|
||||
friend inline Vector4 operator-(const Vector4& a)
|
||||
{ return Vector4(-a.x, -a.y, -a.z, -a.w); }
|
||||
|
||||
// Math operations ignoring the 4th component.
|
||||
friend inline Vector4 Add34(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.x+b.x, a.y+b.y, a.z+b.z); }
|
||||
|
||||
friend inline Vector4 Subtract34(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.x-b.x, a.y-b.y, a.z-b.z); }
|
||||
|
||||
friend inline Vector4 Multiply34(const Vector4& a, float f)
|
||||
{ return Vector4(a.x*f, a.y*f, a.z*f); }
|
||||
|
||||
friend inline Vector4 Multiply34(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.x*b.x, a.y*b.y, a.z*b.z); }
|
||||
|
||||
friend inline Vector4 Divide34(const Vector4& a, float f)
|
||||
{ return Vector4(a.x/f, a.y/f, a.z/f); }
|
||||
|
||||
friend inline Vector4 Negate34(const Vector4& a)
|
||||
{ return Vector4(-a.x, -a.y, -a.z, -a.w); }
|
||||
|
||||
// Dot product.
|
||||
friend inline float Dot3(const Vector4& a, const Vector4& b)
|
||||
{ return a.x*b.x + a.y*b.y + a.z*b.z; }
|
||||
|
||||
friend inline float Dot4(const Vector4& a, const Vector4& b)
|
||||
{ return a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w; }
|
||||
|
||||
// Cross product.
|
||||
friend inline Vector4 Cross3(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x); }
|
||||
|
||||
// Other functions.
|
||||
inline float Length3() const
|
||||
{ return sqrtf(x*x + y*y + z*z); }
|
||||
|
||||
inline void Normalize34()
|
||||
{
|
||||
float len = 1.0f / sqrtf(x*x + y*y + z*z);
|
||||
|
||||
x *= len;
|
||||
y *= len;
|
||||
z *= len;
|
||||
}
|
||||
|
||||
inline void Abs34()
|
||||
{
|
||||
if (x < 0.0f) x = -x;
|
||||
if (y < 0.0f) y = -y;
|
||||
if (z < 0.0f) z = -z;
|
||||
}
|
||||
|
||||
inline void Abs()
|
||||
{
|
||||
if (x < 0.0f) x = -x;
|
||||
if (y < 0.0f) y = -y;
|
||||
if (z < 0.0f) z = -z;
|
||||
if (w < 0.0f) w = -w;
|
||||
}
|
||||
|
||||
protected:
|
||||
float x, y, z, w;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
// ============================================================================
|
||||
// Vector4 class (SSE version).
|
||||
|
||||
#ifdef LC_MATH_SSE
|
||||
|
||||
// If you can't find this file you need to install the VS6 Processor Pack.
|
||||
#include <xmmintrin.h>
|
||||
|
||||
class __declspec(align(16)) Vector4
|
||||
{
|
||||
public:
|
||||
// Constructors.
|
||||
inline Vector4() { }
|
||||
inline explicit Vector4(const __m128& _xyzw)
|
||||
: xyzw(_xyzw) { }
|
||||
inline explicit Vector4(const float _x, const float _y, const float _z)
|
||||
: xyzw(_mm_setr_ps(_x, _y, _z, _z)) { }
|
||||
inline explicit Vector4(const float _x, const float _y, const float _z, const float _w)
|
||||
: xyzw(_mm_setr_ps(_x, _y, _z, _w)) { }
|
||||
|
||||
inline float& operator[](int i) const { return ((const float*)this)[i]; }
|
||||
|
||||
// Comparison.
|
||||
friend inline bool operator==(const Vector4& a, const Vector4& b)
|
||||
{ return !_mm_movemask_ps(_mm_cmpneq_ps(a.xyzw, b.xyzw)); }
|
||||
|
||||
friend inline bool Compare3(const Vector4& a, const Vector4& b)
|
||||
{ return (_mm_movemask_ps(_mm_cmpeq_ps(a.xyzw, b.xyzw)) & 0x7) == 0x7; }
|
||||
|
||||
// Math operations for 4 components.
|
||||
friend inline Vector4 operator+(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(_mm_add_ps(a.xyzw, b.xyzw)); }
|
||||
|
||||
friend inline Vector4 operator-(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(_mm_sub_ps(a.xyzw, b.xyzw)); }
|
||||
|
||||
friend inline Vector4 operator*(const Vector4& a, float f)
|
||||
{ return Vector4(_mm_mul_ps(a.xyzw, _mm_load_ps1(&f))); }
|
||||
|
||||
friend inline Vector4 operator*(const Vector4& a, const Vector4& b)
|
||||
{ return Vector4(_mm_mul_ps(a.xyzw, b.xyzw)); }
|
||||
|
||||
friend inline Vector4 operator/(const Vector4& a, float f)
|
||||
{ return Vector4(_mm_div_ps(a.xyzw, _mm_load_ps1(&f))); }
|
||||
|
||||
friend inline Vector4 operator-(const Vector4& a)
|
||||
{
|
||||
static const __declspec(align(16)) unsigned int Mask[4] = { 0x80000000, 0x80000000, 0x80000000, 0x80000000 }
|
||||
return Vector4(_mm_xor_ps(xyzw, *(__m128*)&Mask));
|
||||
}
|
||||
|
||||
// Math operations ignoring the 4th component.
|
||||
friend inline Vector4 Add34(const Vector4& a, const Vector4& b)
|
||||
{ return a*b }
|
||||
|
||||
friend inline Vector4 Subtract34(const Vector4& a, const Vector4& b)
|
||||
{ return a-b; }
|
||||
|
||||
friend inline Vector4 Multiply34(const Vector4& a, float f)
|
||||
{ return a*f; }
|
||||
|
||||
friend inline Vector4 Multiply34(const Vector4& a, const Vector4& b)
|
||||
{ return a*b; }
|
||||
|
||||
friend inline Vector4 Divide34(const Vector4& a, float f)
|
||||
{ return a/f; }
|
||||
|
||||
friend inline Vector4 Negate34(const Vector4& a)
|
||||
{ return -a; }
|
||||
|
||||
// Dot product.
|
||||
friend inline float Dot3(const Vector4& a, const Vector4& b)
|
||||
{
|
||||
__m128 tmp = _mm_mul_ps(a.xyzw, b.xyzw);
|
||||
__m128 yz = _mm_add_ss(_mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(1, 1, 1, 1)), _mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(2, 2, 2, 2)));
|
||||
tmp = _mm_add_ss(tmp, yz);
|
||||
|
||||
return *(const float*)&tmp;
|
||||
}
|
||||
|
||||
// Cross product.
|
||||
friend inline Vector4 Cross3(const Vector4& a, const Vector4& b)
|
||||
{
|
||||
// a(yzx)*b(zxy)-a(zxy)*b(yzx)
|
||||
__m128 r1 = _mm_mul_ps(_mm_shuffle_ps(a.xyzw, a.xyzw, _MM_SHUFFLE(0, 0, 2, 1)), _mm_shuffle_ps(b.xyzw, b.xyzw, _MM_SHUFFLE(0, 1, 0, 2)));
|
||||
__m128 r2 = _mm_mul_ps(_mm_shuffle_ps(a.xyzw, a.xyzw, _MM_SHUFFLE(0, 1, 0, 2)), _mm_shuffle_ps(b.xyzw, b.xyzw, _MM_SHUFFLE(0, 0, 2, 1)));
|
||||
|
||||
return Vector4(_mm_sub_ps(r1, r2));
|
||||
}
|
||||
|
||||
// Other functions.
|
||||
inline float Length3() const
|
||||
{
|
||||
__m128 tmp = _mm_mul_ps(xyzw, xyzw);
|
||||
__m128 yz = _mm_add_ss(_mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(1, 1, 1, 1)), _mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(2, 2, 2, 2)));
|
||||
tmp = _mm_add_ss(tmp, yz);
|
||||
tmp = _mm_sqrt_ss(tmp);
|
||||
|
||||
return *(const float*)&tmp;
|
||||
}
|
||||
|
||||
inline void Normalize34()
|
||||
{
|
||||
__m128 tmp = _mm_mul_ps(xyzw, xyzw);
|
||||
__m128 yz = _mm_add_ss(_mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(1, 1, 1, 1)), _mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(2, 2, 2, 2)));
|
||||
tmp = _mm_add_ss(tmp, yz);
|
||||
tmp = _mm_rsqrt_ss(tmp);
|
||||
tmp = _mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(0, 0, 0, 0));
|
||||
xyzw = _mm_mul_ps(xyzw, tmp);
|
||||
}
|
||||
|
||||
inline void Abs()
|
||||
{
|
||||
static const __declspec(align(16)) unsigned int Mask[4] = { 0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff }
|
||||
xyzw = _mm_and_ps(xyzw, *(__m128*)&Mask);
|
||||
}
|
||||
|
||||
protected:
|
||||
__m128 xyzw;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
// ============================================================================
|
||||
// 3D Vector class.
|
||||
|
||||
class Vector3
|
||||
{
|
||||
public:
|
||||
// Constructors.
|
||||
inline Vector3()
|
||||
{ }
|
||||
inline explicit Vector3(const Vector4& _v)
|
||||
: m_Value(_v) { }
|
||||
inline explicit Vector3(const float _x, const float _y, const float _z)
|
||||
: m_Value(_x, _y, _z) { }
|
||||
inline explicit Vector3(const float* xyz)
|
||||
: m_Value(xyz[0], xyz[1], xyz[2]) { }
|
||||
|
||||
inline operator const float*() const { return (const float*)this; }
|
||||
inline operator float*() { return (float*)this; }
|
||||
inline const Vector4& GetValue() const { return m_Value; }
|
||||
inline operator const Vector4() const
|
||||
{ return Vector4(m_Value[0], m_Value[1], m_Value[2], 0.0f); }
|
||||
|
||||
inline float& operator[](int i) const { return m_Value[i]; }
|
||||
|
||||
// Math operations.
|
||||
friend inline Vector3 operator+=(Vector3& a, const Vector3& b)
|
||||
{ a.m_Value = a.m_Value + b.m_Value; return a; }
|
||||
|
||||
friend inline Vector3 operator*=(Vector3& a, float b)
|
||||
{ a.m_Value = a.m_Value * b; return a; }
|
||||
|
||||
friend inline Vector3 operator/=(Vector3& a, float b)
|
||||
{ a.m_Value = a.m_Value / b; return a; }
|
||||
|
||||
// Other functions.
|
||||
inline float Length() const
|
||||
{ return m_Value.Length3(); }
|
||||
|
||||
inline float LengthSquared() const
|
||||
{ return Dot3(m_Value, m_Value); }
|
||||
|
||||
inline const Vector3& Normalize()
|
||||
{ m_Value.Normalize34(); return *this; }
|
||||
|
||||
inline void Abs()
|
||||
{ m_Value.Abs34(); }
|
||||
|
||||
protected:
|
||||
Vector4 m_Value;
|
||||
};
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Operators.
|
||||
|
||||
// Comparison.
|
||||
inline bool operator==(const Vector3& a, const Vector3& b)
|
||||
{ return Compare3(a.GetValue(), b.GetValue()); }
|
||||
|
||||
// Multiply by a scalar.
|
||||
inline Vector3 operator*(const Vector3& a, float f)
|
||||
{ return Vector3(Multiply34(a.GetValue(), f)); }
|
||||
|
||||
inline Vector3 operator*(float f, const Vector3& a)
|
||||
{ return Vector3(Multiply34(a.GetValue(), f)); }
|
||||
|
||||
// Divide by a scalar.
|
||||
inline Vector3 operator/(const Vector3& a, float f)
|
||||
{ return Vector3(Divide34(a.GetValue(), f)); }
|
||||
|
||||
inline Vector3 operator/(float f, const Vector3& a)
|
||||
{ return Vector3(Divide34(a.GetValue(), f)); }
|
||||
|
||||
// Add vectors.
|
||||
inline Vector3 operator+(const Vector3& a, const Vector3& b)
|
||||
{ return Vector3(Add34(a.GetValue(), b.GetValue())); }
|
||||
|
||||
// Subtract vectors.
|
||||
inline Vector3 operator-(const Vector3& a, const Vector3& b)
|
||||
{ return Vector3(Subtract34(a.GetValue(), b.GetValue())); }
|
||||
|
||||
// Negate.
|
||||
inline Vector3 operator-(const Vector3& a)
|
||||
{ return Vector3(Negate34(a.GetValue())); }
|
||||
|
||||
// Dot product.
|
||||
inline float Dot3(const Vector3& a, const Vector3& b)
|
||||
{ return Dot3(a.GetValue(), b.GetValue()); }
|
||||
|
||||
// Cross product.
|
||||
inline Vector3 Cross3(const Vector3& a, const Vector3& b)
|
||||
{ return Vector3(Cross3(a.GetValue(), b.GetValue())); }
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Quaternion class.
|
||||
|
||||
class Quaternion
|
||||
{
|
||||
public:
|
||||
// Constructors.
|
||||
inline Quaternion()
|
||||
{ }
|
||||
inline explicit Quaternion(const Vector4& _v)
|
||||
: m_Value(_v) { }
|
||||
inline explicit Quaternion(const float _x, const float _y, const float _z, const float _w)
|
||||
: m_Value(_x, _y, _z, _w) { }
|
||||
|
||||
// Get/Set functions.
|
||||
inline const float operator[](int i) const { return m_Value[i]; }
|
||||
|
||||
// Conversions.
|
||||
inline void FromAxisAngle(const Vector4& AxisAngle)
|
||||
{
|
||||
float s = sinf(AxisAngle[3] / 2.0f);
|
||||
m_Value = Vector4(AxisAngle[0] * s, AxisAngle[1] * s, AxisAngle[2] * s, cosf(AxisAngle[3] / 2.0f));
|
||||
}
|
||||
|
||||
inline void CreateRotationX(float Radians)
|
||||
{
|
||||
m_Value = Vector4(sinf(Radians / 2.0f), 0, 0, cosf(Radians / 2.0f));
|
||||
}
|
||||
|
||||
inline void CreateRotationY(float Radians)
|
||||
{
|
||||
m_Value = Vector4(0, sinf(Radians / 2.0f), 0, cosf(Radians / 2.0f));
|
||||
}
|
||||
|
||||
inline void CreateRotationZ(float Radians)
|
||||
{
|
||||
m_Value = Vector4(0, 0, sinf(Radians / 2.0f), cosf(Radians / 2.0f));
|
||||
}
|
||||
|
||||
inline void ToAxisAngle(Vector4& AxisAngle) const
|
||||
{
|
||||
float Len = m_Value[0]*m_Value[0] + m_Value[1]*m_Value[1] + m_Value[2]*m_Value[2];
|
||||
|
||||
if (Len > 0.0001f)
|
||||
{
|
||||
float f = 1.0f / sqrtf(Len);
|
||||
AxisAngle = Vector4(m_Value[0] * f, m_Value[1] * f, m_Value[2] * f, acosf(m_Value[3]) * 2.0f);
|
||||
}
|
||||
else
|
||||
{
|
||||
AxisAngle = Vector4(0, 0, 1, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Operators.
|
||||
friend inline Quaternion Mul(const Quaternion& a, const Quaternion& b)
|
||||
{
|
||||
float x = a.m_Value[0] * b.m_Value[3] + a.m_Value[1] * b.m_Value[2] - a.m_Value[2] * b.m_Value[1] + a.m_Value[3] * b.m_Value[0];
|
||||
float y = -a.m_Value[0] * b.m_Value[2] + a.m_Value[1] * b.m_Value[3] + a.m_Value[2] * b.m_Value[0] + a.m_Value[3] * b.m_Value[1];
|
||||
float z = a.m_Value[0] * b.m_Value[1] - a.m_Value[1] * b.m_Value[0] + a.m_Value[2] * b.m_Value[3] + a.m_Value[3] * b.m_Value[2];
|
||||
float w = -a.m_Value[0] * b.m_Value[0] - a.m_Value[1] * b.m_Value[1] - a.m_Value[2] * b.m_Value[2] + a.m_Value[3] * b.m_Value[3];
|
||||
|
||||
return Quaternion(x, y, z, w);
|
||||
}
|
||||
|
||||
friend inline Vector3 Mul(const Vector3& a, const Quaternion& b)
|
||||
{
|
||||
// Faster to transform to a matrix and multiply.
|
||||
float Tx = 2.0f*b[0];
|
||||
float Ty = 2.0f*b[1];
|
||||
float Tz = 2.0f*b[2];
|
||||
float Twx = Tx*b[3];
|
||||
float Twy = Ty*b[3];
|
||||
float Twz = Tz*b[3];
|
||||
float Txx = Tx*b[0];
|
||||
float Txy = Ty*b[0];
|
||||
float Txz = Tz*b[0];
|
||||
float Tyy = Ty*b[1];
|
||||
float Tyz = Tz*b[1];
|
||||
float Tzz = Tz*b[2];
|
||||
|
||||
Vector3 Rows[3];
|
||||
Rows[0] = Vector3(1.0f-(Tyy+Tzz), Txy+Twz, Txz-Twy);
|
||||
Rows[1] = Vector3(Txy-Twz, 1.0f-(Txx+Tzz), Tyz+Twx);
|
||||
Rows[2] = Vector3(Txz+Twy, Tyz-Twx, 1.0f-(Txx+Tyy));
|
||||
|
||||
return Vector3(Rows[0].GetValue()*a[0] + Rows[1].GetValue()*a[1] + Rows[2].GetValue()*a[2]);
|
||||
}
|
||||
|
||||
protected:
|
||||
Vector4 m_Value;
|
||||
};
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// 3x3 Matrix class.
|
||||
|
||||
class Matrix33
|
||||
{
|
||||
public:
|
||||
// Constructors.
|
||||
inline Matrix33()
|
||||
{ }
|
||||
inline Matrix33(const Vector3& Row0, const Vector3& Row1, const Vector3& Row2)
|
||||
{ m_Rows[0] = Row0; m_Rows[1] = Row1; m_Rows[2] = Row2; }
|
||||
|
||||
inline void LoadIdentity()
|
||||
{
|
||||
m_Rows[0] = Vector3(1.0f, 0.0f, 0.0f);
|
||||
m_Rows[1] = Vector3(0.0f, 1.0f, 0.0f);
|
||||
m_Rows[2] = Vector3(0.0f, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
inline void CreateFromAxisAngle(const Vector3& Axis, const float Radians)
|
||||
{
|
||||
float s, c, mag, xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
|
||||
|
||||
s = sinf(Radians);
|
||||
c = cosf(Radians);
|
||||
mag = Axis.Length();
|
||||
|
||||
if (mag == 0.0f)
|
||||
{
|
||||
LoadIdentity();
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 Normal = Axis * (1.0f / mag);
|
||||
|
||||
xx = Normal[0] * Normal[0];
|
||||
yy = Normal[1] * Normal[1];
|
||||
zz = Normal[2] * Normal[2];
|
||||
xy = Normal[0] * Normal[1];
|
||||
yz = Normal[1] * Normal[2];
|
||||
zx = Normal[2] * Normal[0];
|
||||
xs = Normal[0] * s;
|
||||
ys = Normal[1] * s;
|
||||
zs = Normal[2] * s;
|
||||
one_c = 1.0f - c;
|
||||
|
||||
m_Rows[0] = Vector3((one_c * xx) + c, (one_c * xy) + zs, (one_c * zx) - ys);
|
||||
m_Rows[1] = Vector3((one_c * xy) - zs, (one_c * yy) + c, (one_c * yz) + xs);
|
||||
m_Rows[2] = Vector3((one_c * zx) + ys, (one_c * yz) - xs, (one_c * zz) + c);
|
||||
}
|
||||
|
||||
friend inline Vector3 Mul(const Vector3& a, const Matrix33& b)
|
||||
{ return Vector3(b.m_Rows[0]*a[0] + b.m_Rows[1]*a[1] + b.m_Rows[2]*a[2]); }
|
||||
|
||||
protected:
|
||||
Vector3 m_Rows[3];
|
||||
|
||||
friend class Matrix44;
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// 4x4 Matrix class.
|
||||
|
||||
class Matrix44
|
||||
{
|
||||
public:
|
||||
inline Matrix44()
|
||||
{ }
|
||||
inline Matrix44(const Vector4& Row0, const Vector4& Row1, const Vector4& Row2, const Vector4& Row3)
|
||||
{ m_Rows[0] = Row0; m_Rows[1] = Row1; m_Rows[2] = Row2; m_Rows[3] = Row3; }
|
||||
|
||||
inline operator const float*() const { return (const float*)this; }
|
||||
inline const Vector4& operator[](int i) const { return m_Rows[i]; }
|
||||
inline Vector4& operator[](int i) { return m_Rows[i]; }
|
||||
|
||||
inline void LoadIdentity()
|
||||
{
|
||||
m_Rows[0] = Vector4(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
m_Rows[1] = Vector4(0.0f, 1.0f, 0.0f, 0.0f);
|
||||
m_Rows[2] = Vector4(0.0f, 0.0f, 1.0f, 0.0f);
|
||||
m_Rows[3] = Vector4(0.0f, 0.0f, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
// Math operations.
|
||||
friend inline Vector3 Mul31(const Vector3& a, const Matrix44& b)
|
||||
{ return Vector3(b.m_Rows[0]*a[0] + b.m_Rows[1]*a[1] + b.m_Rows[2]*a[2] + b.m_Rows[3]); }
|
||||
|
||||
friend inline Vector3 Mul30(const Vector3& a, const Matrix44& b)
|
||||
{ return Vector3(b.m_Rows[0]*a[0] + b.m_Rows[1]*a[1] + b.m_Rows[2]*a[2]); }
|
||||
|
||||
friend inline Vector4 Mul4(const Vector4& a, const Matrix44& b)
|
||||
{ return Vector4(b.m_Rows[0]*a[0] + b.m_Rows[1]*a[1] + b.m_Rows[2]*a[2] + b.m_Rows[3]*a[3]); }
|
||||
|
||||
friend inline Matrix44 Mul(const Matrix44& a, const Matrix44& b)
|
||||
{
|
||||
Vector4 Col0(b.m_Rows[0][0], b.m_Rows[1][0], b.m_Rows[2][0], b.m_Rows[3][0]);
|
||||
Vector4 Col1(b.m_Rows[0][1], b.m_Rows[1][1], b.m_Rows[2][1], b.m_Rows[3][1]);
|
||||
Vector4 Col2(b.m_Rows[0][2], b.m_Rows[1][2], b.m_Rows[2][2], b.m_Rows[3][2]);
|
||||
Vector4 Col3(b.m_Rows[0][3], b.m_Rows[1][3], b.m_Rows[2][3], b.m_Rows[3][3]);
|
||||
|
||||
Vector4 Ret0(Dot4(a.m_Rows[0], Col0), Dot4(a.m_Rows[0], Col1), Dot4(a.m_Rows[0], Col2), Dot4(a.m_Rows[0], Col3));
|
||||
Vector4 Ret1(Dot4(a.m_Rows[1], Col0), Dot4(a.m_Rows[1], Col1), Dot4(a.m_Rows[1], Col2), Dot4(a.m_Rows[1], Col3));
|
||||
Vector4 Ret2(Dot4(a.m_Rows[2], Col0), Dot4(a.m_Rows[2], Col1), Dot4(a.m_Rows[2], Col2), Dot4(a.m_Rows[2], Col3));
|
||||
Vector4 Ret3(Dot4(a.m_Rows[3], Col0), Dot4(a.m_Rows[3], Col1), Dot4(a.m_Rows[3], Col2), Dot4(a.m_Rows[3], Col3));
|
||||
|
||||
return Matrix44(Ret0, Ret1, Ret2, Ret3);
|
||||
}
|
||||
|
||||
inline Matrix44& operator=(const Matrix33& a)
|
||||
{
|
||||
m_Rows[0] = Vector4(a.m_Rows[0][0], a.m_Rows[0][1], a.m_Rows[0][2], 0.0f);
|
||||
m_Rows[1] = Vector4(a.m_Rows[1][0], a.m_Rows[1][1], a.m_Rows[1][2], 0.0f);
|
||||
m_Rows[2] = Vector4(a.m_Rows[2][0], a.m_Rows[2][1], a.m_Rows[2][2], 0.0f);
|
||||
m_Rows[3] = Vector4(0.0f, 0.0f, 0.0f, 1.0f);
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline void Transpose3()
|
||||
{
|
||||
Vector4 a = m_Rows[0], b = m_Rows[1], c = m_Rows[2];
|
||||
m_Rows[0] = Vector4(a[0], b[0], c[0], a[3]);
|
||||
m_Rows[1] = Vector4(a[1], b[1], c[1], b[3]);
|
||||
m_Rows[2] = Vector4(a[2], b[2], c[2], c[3]);
|
||||
}
|
||||
|
||||
inline void SetTranslation(const Vector3& a)
|
||||
{ m_Rows[3] = Vector4(a[0], a[1], a[2], 1.0f); }
|
||||
|
||||
friend Matrix44 Inverse(const Matrix44& m);
|
||||
void CreateLookAt(const Vector3& Eye, const Vector3& Target, const Vector3& Up);
|
||||
void CreatePerspective(float FoVy, float Aspect, float Near, float Far);
|
||||
void CreateOrtho(float Left, float Right, float Bottom, float Top, float Near, float Far);
|
||||
|
||||
void CreateFromAxisAngle(const Vector3& Axis, float Radians)
|
||||
{
|
||||
Matrix33 Mat;
|
||||
Mat.CreateFromAxisAngle(Axis, Radians);
|
||||
*this = Mat;
|
||||
}
|
||||
|
||||
Vector4 ToAxisAngle()
|
||||
{
|
||||
Matrix33 tmp(Vector3(m_Rows[0]).Normalize(), Vector3(m_Rows[1]).Normalize(), Vector3(m_Rows[2]).Normalize());
|
||||
|
||||
// Determinant should be 1 for rotation matrices.
|
||||
float Determinant = tmp.m_Rows[0][0] * tmp.m_Rows[1][1] * tmp.m_Rows[2][2] + tmp.m_Rows[0][1] * tmp.m_Rows[1][2] * tmp.m_Rows[2][0] +
|
||||
tmp.m_Rows[0][2] * tmp.m_Rows[1][0] * tmp.m_Rows[2][1] - tmp.m_Rows[0][0] * tmp.m_Rows[1][2] * tmp.m_Rows[2][1] -
|
||||
tmp.m_Rows[0][1] * tmp.m_Rows[1][0] * tmp.m_Rows[2][2] - tmp.m_Rows[0][2] * tmp.m_Rows[1][1] * tmp.m_Rows[2][0];
|
||||
|
||||
if (Determinant < 0.0f)
|
||||
tmp.m_Rows[0] *= -1.0f;
|
||||
|
||||
float Trace = tmp.m_Rows[0][0] + tmp.m_Rows[1][1] + tmp.m_Rows[2][2];
|
||||
float Cos = 0.5f * (Trace - 1.0f);
|
||||
Vector4 rot;
|
||||
|
||||
if (Cos < -1.0f)
|
||||
Cos = -1.0f;
|
||||
else if (Cos > 1.0f)
|
||||
Cos = 1.0f;
|
||||
rot[3] = acosf(Cos); // in [0,PI]
|
||||
|
||||
if (rot[3] > 0.01f)
|
||||
{
|
||||
if (fabsf(3.141592f - rot[3]) > 0.01f)
|
||||
{
|
||||
rot[0] = tmp.m_Rows[1][2] - tmp.m_Rows[2][1];
|
||||
rot[1] = tmp.m_Rows[2][0] - tmp.m_Rows[0][2];
|
||||
rot[2] = tmp.m_Rows[0][1] - tmp.m_Rows[1][0];
|
||||
|
||||
float inv = 1.0f / sqrtf(rot[0]*rot[0] + rot[1]*rot[1] + rot[2]*rot[2]);
|
||||
|
||||
rot[0] *= inv;
|
||||
rot[1] *= inv;
|
||||
rot[2] *= inv;
|
||||
}
|
||||
else
|
||||
{
|
||||
// angle is PI
|
||||
float HalfInverse;
|
||||
if (tmp.m_Rows[0][0] >= tmp.m_Rows[1][1])
|
||||
{
|
||||
// r00 >= r11
|
||||
if (tmp.m_Rows[0][0] >= tmp.m_Rows[2][2])
|
||||
{
|
||||
// r00 is maximum diagonal term
|
||||
rot[0] = 0.5f * sqrtf(tmp.m_Rows[0][0] - tmp.m_Rows[1][1] - tmp.m_Rows[2][2] + 1.0f);
|
||||
HalfInverse = 0.5f / rot[0];
|
||||
rot[1] = HalfInverse * tmp.m_Rows[1][0];
|
||||
rot[2] = HalfInverse * tmp.m_Rows[2][0];
|
||||
}
|
||||
else
|
||||
{
|
||||
// r22 is maximum diagonal term
|
||||
rot[2] = 0.5f * sqrtf(tmp.m_Rows[2][2] - tmp.m_Rows[0][0] - tmp.m_Rows[1][1] + 1.0f);
|
||||
HalfInverse = 0.5f / rot[2];
|
||||
rot[0] = HalfInverse * tmp.m_Rows[2][0];
|
||||
rot[1] = HalfInverse * tmp.m_Rows[2][1];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// r11 > r00
|
||||
if (tmp.m_Rows[1][1] >= tmp.m_Rows[2][2])
|
||||
{
|
||||
// r11 is maximum diagonal term
|
||||
rot[1] = 0.5f * sqrtf(tmp.m_Rows[1][1] - tmp.m_Rows[0][0] - tmp.m_Rows[2][2] + 1.0f);
|
||||
HalfInverse = 0.5f / rot[1];
|
||||
rot[0] = HalfInverse * tmp.m_Rows[1][0];
|
||||
rot[2] = HalfInverse * tmp.m_Rows[2][1];
|
||||
}
|
||||
else
|
||||
{
|
||||
// r22 is maximum diagonal term
|
||||
rot[2] = 0.5f * sqrtf(tmp.m_Rows[2][2] - tmp.m_Rows[0][0] - tmp.m_Rows[1][1] + 1.0f);
|
||||
HalfInverse = 0.5f / rot[2];
|
||||
rot[0] = HalfInverse * tmp.m_Rows[2][0];
|
||||
rot[1] = HalfInverse * tmp.m_Rows[2][1];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// The angle is 0 and the matrix is the identity.
|
||||
rot[0] = 0.0f;
|
||||
rot[1] = 0.0f;
|
||||
rot[2] = 1.0f;
|
||||
}
|
||||
|
||||
return rot;
|
||||
}
|
||||
|
||||
protected:
|
||||
Vector4 m_Rows[4];
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Linear Algebra Functions.
|
||||
|
||||
Vector3 ZoomExtents(const Vector3& Position, const Matrix44& WorldView, const Matrix44& Projection, const Vector3* Points, int NumPoints);
|
||||
|
||||
Vector3 ProjectPoint(const Vector3& Point, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4]);
|
||||
void ProjectPoints(Vector3* Points, int NumPoints, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4]);
|
||||
Vector3 UnprojectPoint(const Vector3& Point, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4]);
|
||||
void UnprojectPoints(Vector3* Points, int NumPoints, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4]);
|
||||
|
||||
void PolygonPlaneClip(Vector3* InPoints, int NumInPoints, Vector3* OutPoints, int* NumOutPoints, const Vector4& Plane);
|
||||
bool LinePlaneIntersection(Vector3& Intersection, const Vector3& Start, const Vector3& End, const Vector4& Plane);
|
||||
bool LineTriangleMinIntersection(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& Start, const Vector3& End, float& MinDist, Vector3& Intersection);
|
||||
bool LineQuadMinIntersection(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, const Vector3& Start, const Vector3& End, float& MinDist, Vector3& Intersection);
|
||||
|
||||
#endif
|
||||
388
common/lc_math.h
388
common/lc_math.h
|
|
@ -32,11 +32,6 @@ inline T lcClamp(const T& Value, const U& Min, const V& Max)
|
|||
return Value;
|
||||
}
|
||||
|
||||
class lcVector3;
|
||||
class lcVector4;
|
||||
class lcMatrix33;
|
||||
class lcMatrix44;
|
||||
|
||||
class lcVector3
|
||||
{
|
||||
public:
|
||||
|
|
@ -282,6 +277,11 @@ inline float lcVector3::LengthSquared() const
|
|||
return x * x + y * y + z * z;
|
||||
}
|
||||
|
||||
inline float lcLength(const lcVector3& a)
|
||||
{
|
||||
return a.Length();
|
||||
}
|
||||
|
||||
inline lcVector3 lcNormalize(const lcVector3& a)
|
||||
{
|
||||
lcVector3 Ret(a);
|
||||
|
|
@ -304,6 +304,11 @@ inline float lcDot3(const lcVector3& a, const lcVector4& b)
|
|||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
}
|
||||
|
||||
inline float lcDot3(const lcVector4& a, const lcVector4& b)
|
||||
{
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
}
|
||||
|
||||
inline float lcDot(const lcVector4& a, const lcVector4& b)
|
||||
{
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
|
|
@ -855,6 +860,76 @@ inline lcMatrix44 lcMatrix44Inverse(const lcMatrix44& m)
|
|||
#undef SWAP_ROWS
|
||||
}
|
||||
|
||||
inline lcVector4 lcQuaternionRotationX(float Radians)
|
||||
{
|
||||
return lcVector4(sinf(Radians / 2.0f), 0, 0, cosf(Radians / 2.0f));
|
||||
}
|
||||
|
||||
inline lcVector4 lcQuaternionRotationY(float Radians)
|
||||
{
|
||||
return lcVector4(0, sinf(Radians / 2.0f), 0, cosf(Radians / 2.0f));
|
||||
}
|
||||
|
||||
inline lcVector4 lcQuaternionRotationZ(float Radians)
|
||||
{
|
||||
return lcVector4(0, 0, sinf(Radians / 2.0f), cosf(Radians / 2.0f));
|
||||
}
|
||||
|
||||
inline lcVector4 lcQuaternionFromAxisAngle(const lcVector4& a)
|
||||
{
|
||||
float s = sinf(a[3] / 2.0f);
|
||||
return lcVector4(a[0] * s, a[1] * s, a[2] * s, cosf(a[3] / 2.0f));
|
||||
}
|
||||
|
||||
inline lcVector4 lcQuaternionToAxisAngle(const lcVector4& a)
|
||||
{
|
||||
float Len = lcDot3(a, a);
|
||||
|
||||
if (Len > 0.00001f)
|
||||
{
|
||||
float f = 1.0f / sqrtf(Len);
|
||||
return lcVector4(a[0] * f, a[1] * f, a[2] * f, acosf(a[3]) * 2.0f);
|
||||
}
|
||||
else
|
||||
{
|
||||
return lcVector4(0, 0, 1, 0);
|
||||
}
|
||||
}
|
||||
|
||||
inline lcVector4 lcQuaternionMultiply(const lcVector4& a, const lcVector4& b)
|
||||
{
|
||||
float x = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
|
||||
float y = -a[0] * b[2] + a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
|
||||
float z = a[0] * b[1] - a[1] * b[0] + a[2] * b[3] + a[3] * b[2];
|
||||
float w = -a[0] * b[0] - a[1] * b[1] - a[2] * b[2] + a[3] * b[3];
|
||||
|
||||
return lcVector4(x, y, z, w);
|
||||
}
|
||||
|
||||
inline lcVector3 lcQuaternionMul(const lcVector3& a, const lcVector4& b)
|
||||
{
|
||||
// Faster to transform to a matrix and multiply.
|
||||
float Tx = 2.0f*b[0];
|
||||
float Ty = 2.0f*b[1];
|
||||
float Tz = 2.0f*b[2];
|
||||
float Twx = Tx*b[3];
|
||||
float Twy = Ty*b[3];
|
||||
float Twz = Tz*b[3];
|
||||
float Txx = Tx*b[0];
|
||||
float Txy = Ty*b[0];
|
||||
float Txz = Tz*b[0];
|
||||
float Tyy = Ty*b[1];
|
||||
float Tyz = Tz*b[1];
|
||||
float Tzz = Tz*b[2];
|
||||
|
||||
lcVector3 Rows[3];
|
||||
Rows[0] = lcVector3(1.0f-(Tyy+Tzz), Txy+Twz, Txz-Twy);
|
||||
Rows[1] = lcVector3(Txy-Twz, 1.0f-(Txx+Tzz), Tyz+Twx);
|
||||
Rows[2] = lcVector3(Txz+Twy, Tyz-Twx, 1.0f-(Txx+Tyy));
|
||||
|
||||
return lcVector3(Rows[0]*a[0] + Rows[1]*a[1] + Rows[2]*a[2]);
|
||||
}
|
||||
|
||||
// Convert world coordinates to screen coordinates.
|
||||
inline lcVector3 lcProjectPoint(const lcVector3& Point, const lcMatrix44& ModelView, const lcMatrix44& Projection, const int Viewport[4])
|
||||
{
|
||||
|
|
@ -986,7 +1061,7 @@ inline lcVector3 lcZoomExtents(const lcVector3& Position, const lcMatrix44& Worl
|
|||
|
||||
// Calculate the intersection of a line segment and a plane and returns false
|
||||
// if they are parallel or the intersection is outside the line segment.
|
||||
inline bool lcLinePlaneIntersection(lcVector3& Intersection, const lcVector3& Start, const lcVector3& End, const lcVector4& Plane)
|
||||
inline bool lcLinePlaneIntersection(lcVector3* Intersection, const lcVector3& Start, const lcVector3& End, const lcVector4& Plane)
|
||||
{
|
||||
lcVector3 Dir = End - Start;
|
||||
lcVector3 PlaneNormal(Plane[0], Plane[1], Plane[2]);
|
||||
|
|
@ -999,7 +1074,7 @@ inline bool lcLinePlaneIntersection(lcVector3& Intersection, const lcVector3& St
|
|||
|
||||
float t = -t1 / t2;
|
||||
|
||||
Intersection = Start + t * Dir;
|
||||
*Intersection = Start + t * Dir;
|
||||
|
||||
if ((t < 0.0f) || (t > 1.0f))
|
||||
return false;
|
||||
|
|
@ -1007,7 +1082,7 @@ inline bool lcLinePlaneIntersection(lcVector3& Intersection, const lcVector3& St
|
|||
return true;
|
||||
}
|
||||
|
||||
inline bool lcLineTriangleMinIntersection(const lcVector3& p1, const lcVector3& p2, const lcVector3& p3, const lcVector3& Start, const lcVector3& End, float& MinDist, lcVector3& Intersection)
|
||||
inline bool lcLineTriangleMinIntersection(const lcVector3& p1, const lcVector3& p2, const lcVector3& p3, const lcVector3& Start, const lcVector3& End, float* MinDist, lcVector3* Intersection)
|
||||
{
|
||||
// Calculate the polygon plane.
|
||||
lcVector3 PlaneNormal = lcCross(p1 - p2, p3 - p2);
|
||||
|
|
@ -1028,18 +1103,18 @@ inline bool lcLineTriangleMinIntersection(const lcVector3& p1, const lcVector3&
|
|||
return false;
|
||||
|
||||
// Intersection of the plane and line segment.
|
||||
Intersection = Start - (t1 / t2) * Dir;
|
||||
*Intersection = Start - (t1 / t2) * Dir;
|
||||
|
||||
float Dist = (Start - Intersection).Length();
|
||||
float Dist = lcLength(Start - *Intersection);
|
||||
|
||||
if (Dist > MinDist)
|
||||
if (Dist > *MinDist)
|
||||
return false;
|
||||
|
||||
// Check if we're inside the triangle.
|
||||
lcVector3 pa1, pa2, pa3;
|
||||
pa1 = lcNormalize(p1 - Intersection);
|
||||
pa2 = lcNormalize(p2 - Intersection);
|
||||
pa3 = lcNormalize(p3 - Intersection);
|
||||
pa1 = lcNormalize(p1 - *Intersection);
|
||||
pa2 = lcNormalize(p2 - *Intersection);
|
||||
pa3 = lcNormalize(p3 - *Intersection);
|
||||
|
||||
float a1, a2, a3;
|
||||
a1 = lcDot(pa1, pa2);
|
||||
|
|
@ -1050,11 +1125,294 @@ inline bool lcLineTriangleMinIntersection(const lcVector3& p1, const lcVector3&
|
|||
|
||||
if (fabs(total - 360) <= 0.001f)
|
||||
{
|
||||
MinDist = Dist;
|
||||
*MinDist = Dist;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sutherland-Hodgman method of clipping a polygon to a plane.
|
||||
inline void lcPolygonPlaneClip(lcVector3* InPoints, int NumInPoints, lcVector3* OutPoints, int* NumOutPoints, const lcVector4& Plane)
|
||||
{
|
||||
lcVector3 *s, *p, i;
|
||||
|
||||
*NumOutPoints = 0;
|
||||
s = &InPoints[NumInPoints-1];
|
||||
|
||||
for (int j = 0; j < NumInPoints; j++)
|
||||
{
|
||||
p = &InPoints[j];
|
||||
|
||||
if (lcDot3(*p, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
if (lcDot3(*s, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
// Both points inside.
|
||||
OutPoints[*NumOutPoints] = *p;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Outside, inside.
|
||||
lcLinePlaneIntersection(&i, *s, *p, Plane);
|
||||
|
||||
OutPoints[*NumOutPoints] = i;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
OutPoints[*NumOutPoints] = *p;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lcDot3(*s, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
// Inside, outside.
|
||||
lcLinePlaneIntersection(&i, *s, *p, Plane);
|
||||
|
||||
OutPoints[*NumOutPoints] = i;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
}
|
||||
|
||||
s = p;
|
||||
}
|
||||
}
|
||||
|
||||
// Return true if a polygon intersects a set of planes.
|
||||
inline bool lcTriangleIntersectsPlanes(float* p1, float* p2, float* p3, const lcVector4 Planes[6])
|
||||
{
|
||||
const int NumPlanes = 6;
|
||||
float* Points[3] = { p1, p2, p3 };
|
||||
int Outcodes[3] = { 0, 0, 0 }, i;
|
||||
int NumPoints = 3;
|
||||
|
||||
// First do the Cohen-Sutherland out code test for trivial rejects/accepts.
|
||||
for (i = 0; i < NumPoints; i++)
|
||||
{
|
||||
lcVector3 Pt(Points[i][0], Points[i][1], Points[i][2]);
|
||||
|
||||
for (int j = 0; j < NumPlanes; j++)
|
||||
{
|
||||
if (lcDot3(Pt, Planes[j]) + Planes[j][3] > 0)
|
||||
Outcodes[i] |= 1 << j;
|
||||
}
|
||||
}
|
||||
|
||||
// Polygon completely outside a plane.
|
||||
if ((Outcodes[0] & Outcodes[1] & Outcodes[2]) != 0)
|
||||
return false;
|
||||
|
||||
// If any vertex has an out code of all zeros then we intersect the volume.
|
||||
if (!Outcodes[0] || !Outcodes[1] || !Outcodes[2])
|
||||
return true;
|
||||
|
||||
// Buffers for clipping the polygon.
|
||||
lcVector3 ClipPoints[2][8];
|
||||
int NumClipPoints[2];
|
||||
int ClipBuffer = 0;
|
||||
|
||||
NumClipPoints[0] = NumPoints;
|
||||
ClipPoints[0][0] = lcVector3(p1[0], p1[1], p1[2]);
|
||||
ClipPoints[0][1] = lcVector3(p2[0], p2[1], p2[2]);
|
||||
ClipPoints[0][2] = lcVector3(p3[0], p3[1], p3[2]);
|
||||
|
||||
// Now clip the polygon against the planes.
|
||||
for (i = 0; i < NumPlanes; i++)
|
||||
{
|
||||
lcPolygonPlaneClip(ClipPoints[ClipBuffer], NumClipPoints[ClipBuffer], ClipPoints[ClipBuffer^1], &NumClipPoints[ClipBuffer^1], Planes[i]);
|
||||
ClipBuffer ^= 1;
|
||||
|
||||
if (!NumClipPoints[ClipBuffer])
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
float LinePointMinDistance(const Vector3& Point, const Vector3& Start, const Vector3& End)
|
||||
{
|
||||
Vector3 Dir = End - Start;
|
||||
|
||||
float t1 = Dot3(Start - Point, Dir);
|
||||
float t2 = LengthSquared(Dir);
|
||||
|
||||
float t = -t1 / t2;
|
||||
|
||||
if (t < 0.0f)
|
||||
t = 0.0f;
|
||||
else if (t > 1.0f)
|
||||
t = 1.0f;
|
||||
|
||||
Vector3 Closest = Start + t * Dir;
|
||||
|
||||
return Length(Closest - Point);
|
||||
}
|
||||
|
||||
// Return true if a ray intersects a bounding box, and calculates the distance from the start of the ray (adapted from Graphics Gems).
|
||||
bool BoundingBoxRayMinIntersectDistance(const BoundingBox& Box, const Vector3& Start, const Vector3& End, float* Dist, Vector3* Intersection)
|
||||
{
|
||||
bool MiddleQuadrant[3];
|
||||
bool Inside = true;
|
||||
float CandidatePlane[3];
|
||||
float MaxT[3];
|
||||
int i;
|
||||
|
||||
// Find candidate planes.
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if (Start[i] < Box.m_Min[i])
|
||||
{
|
||||
MiddleQuadrant[i] = false;
|
||||
CandidatePlane[i] = Box.m_Min[i];
|
||||
Inside = false;
|
||||
}
|
||||
else if (Start[i] > Box.m_Max[i])
|
||||
{
|
||||
MiddleQuadrant[i] = false;
|
||||
CandidatePlane[i] = Box.m_Max[i];
|
||||
Inside = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
MiddleQuadrant[i] = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Ray origin inside box.
|
||||
if (Inside)
|
||||
{
|
||||
*Dist = 0;
|
||||
|
||||
if (*Intersection)
|
||||
*Intersection = Start;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Calculate T distances to candidate planes.
|
||||
Vector3 Dir = End - Start;
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if (!MiddleQuadrant[i] && Dir[i] != 0.0f)
|
||||
MaxT[i] = (CandidatePlane[i] - Start[i]) / Dir[i];
|
||||
else
|
||||
MaxT[i] = -1.0f;
|
||||
}
|
||||
|
||||
// Get largest of the MaxT's for final choice of intersection.
|
||||
int WhichPlane = 0;
|
||||
for (i = 1; i < 3; i++)
|
||||
if (MaxT[WhichPlane] < MaxT[i])
|
||||
WhichPlane = i;
|
||||
|
||||
// Check final candidate actually inside box.
|
||||
if (MaxT[WhichPlane] < 0.0f)
|
||||
return false;
|
||||
|
||||
Vector3 Point;
|
||||
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if (WhichPlane != i)
|
||||
{
|
||||
Point[i] = Start[i] + MaxT[WhichPlane] * Dir[i];
|
||||
if (Point[i] < Box.m_Min[i] || Point[i] > Box.m_Max[i])
|
||||
return false;
|
||||
}
|
||||
else
|
||||
Point[i] = CandidatePlane[i];
|
||||
}
|
||||
|
||||
*Dist = Length(Point - Start);
|
||||
|
||||
if (*Intersection)
|
||||
*Intersection = Point;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Return true if Box intersects the volume defined by Planes.
|
||||
bool BoundingBoxIntersectsVolume(const BoundingBox& Box, const Vector4* Planes, int NumPlanes)
|
||||
{
|
||||
Vector3 Points[8];
|
||||
Box.GetPoints(Points);
|
||||
|
||||
// Start by testing trivial reject/accept cases.
|
||||
int Outcodes[8];
|
||||
int i;
|
||||
|
||||
for (i = 0; i < 8; i++)
|
||||
{
|
||||
Outcodes[i] = 0;
|
||||
|
||||
for (int j = 0; j < NumPlanes; j++)
|
||||
{
|
||||
if (Dot3(Points[i], Planes[j]) + Planes[j][3] > 0)
|
||||
Outcodes[i] |= 1 << j;
|
||||
}
|
||||
}
|
||||
|
||||
int OutcodesOR = 0, OutcodesAND = 0x3f;
|
||||
|
||||
for (i = 0; i < 8; i++)
|
||||
{
|
||||
OutcodesAND &= Outcodes[i];
|
||||
OutcodesOR |= Outcodes[i];
|
||||
}
|
||||
|
||||
// All corners outside the same plane.
|
||||
if (OutcodesAND != 0)
|
||||
return false;
|
||||
|
||||
// All corners inside the volume.
|
||||
if (OutcodesOR == 0)
|
||||
return true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SphereRayMinIntersectDistance(const Vector3& Center, float Radius, const Vector3& Start, const Vector3& End, float* Dist)
|
||||
{
|
||||
Vector3 Dir = Center - Start;
|
||||
float LengthSquaredDir = LengthSquared(Dir);
|
||||
float RadiusSquared = Radius * Radius;
|
||||
|
||||
if (LengthSquaredDir < RadiusSquared)
|
||||
{
|
||||
// Ray origin inside sphere.
|
||||
*Dist = 0;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector3 RayDir = End - Start;
|
||||
float t = Dot3(Dir, RayDir) / LengthSquared(RayDir);
|
||||
|
||||
// Ray points away from sphere.
|
||||
if (t < 0)
|
||||
return false;
|
||||
|
||||
float c = (RadiusSquared - LengthSquaredDir) / LengthSquared(RayDir) + (t * t);
|
||||
if (c > 0)
|
||||
{
|
||||
*Dist = t - sqrtf(c);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool SphereIntersectsVolume(const Vector3& Center, float Radius, const Vector4* Planes, int NumPlanes)
|
||||
{
|
||||
for (int j = 0; j < NumPlanes; j++)
|
||||
if (Dot3(Center, Planes[j]) + Planes[j][3] > Radius)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}*/
|
||||
|
||||
#endif // _LC_MATH_H_
|
||||
|
|
|
|||
|
|
@ -200,7 +200,7 @@ bool lcMesh::MinIntersectDist(const lcVector3& Start, const lcVector3& End, floa
|
|||
lcVector3 v2(p2[0], p2[1], p2[2]);
|
||||
lcVector3 v3(p3[0], p3[1], p3[2]);
|
||||
|
||||
if (lcLineTriangleMinIntersection(v1, v2, v3, Start, End, MinDist, Intersection))
|
||||
if (lcLineTriangleMinIntersection(v1, v2, v3, Start, End, &MinDist, &Intersection))
|
||||
Hit = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -216,120 +216,6 @@ bool lcMesh::MinIntersectDist(const lcVector3& Start, const lcVector3& End, floa
|
|||
return MinIntersectDist<GLuint>(Start, End, MinDist, Intersection);
|
||||
}
|
||||
|
||||
// Sutherland-Hodgman method of clipping a polygon to a plane.
|
||||
static void lcPolygonPlaneClip(lcVector3* InPoints, int NumInPoints, lcVector3* OutPoints, int* NumOutPoints, const lcVector4& Plane)
|
||||
{
|
||||
lcVector3 *s, *p, i;
|
||||
|
||||
*NumOutPoints = 0;
|
||||
s = &InPoints[NumInPoints-1];
|
||||
|
||||
for (int j = 0; j < NumInPoints; j++)
|
||||
{
|
||||
p = &InPoints[j];
|
||||
|
||||
if (lcDot3(*p, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
if (lcDot3(*s, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
// Both points inside.
|
||||
OutPoints[*NumOutPoints] = *p;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Outside, inside.
|
||||
lcLinePlaneIntersection(i, *s, *p, Plane);
|
||||
|
||||
OutPoints[*NumOutPoints] = i;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
OutPoints[*NumOutPoints] = *p;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lcDot3(*s, Plane) + Plane[3] <= 0)
|
||||
{
|
||||
// Inside, outside.
|
||||
lcLinePlaneIntersection(i, *s, *p, Plane);
|
||||
|
||||
OutPoints[*NumOutPoints] = i;
|
||||
*NumOutPoints = *NumOutPoints + 1;
|
||||
}
|
||||
}
|
||||
|
||||
s = p;
|
||||
}
|
||||
}
|
||||
|
||||
// Return true if a polygon intersects a set of planes.
|
||||
static bool PolygonIntersectsPlanes(float* p1, float* p2, float* p3, float* p4, const lcVector4 Planes[6]) // TODO: move to lc_math
|
||||
{
|
||||
const int NumPlanes = 6;
|
||||
float* Points[4] = { p1, p2, p3, p4 };
|
||||
int Outcodes[4] = { 0, 0, 0, 0 }, i;
|
||||
int NumPoints = (p4 != NULL) ? 4 : 3;
|
||||
|
||||
// First do the Cohen-Sutherland out code test for trivial rejects/accepts.
|
||||
for (i = 0; i < NumPoints; i++)
|
||||
{
|
||||
lcVector3 Pt(Points[i][0], Points[i][1], Points[i][2]);
|
||||
|
||||
for (int j = 0; j < NumPlanes; j++)
|
||||
{
|
||||
if (lcDot3(Pt, Planes[j]) + Planes[j][3] > 0)
|
||||
Outcodes[i] |= 1 << j;
|
||||
}
|
||||
}
|
||||
|
||||
if (p4 != NULL)
|
||||
{
|
||||
// Polygon completely outside a plane.
|
||||
if ((Outcodes[0] & Outcodes[1] & Outcodes[2] & Outcodes[3]) != 0)
|
||||
return false;
|
||||
|
||||
// If any vertex has an out code of all zeros then we intersect the volume.
|
||||
if (!Outcodes[0] || !Outcodes[1] || !Outcodes[2] || !Outcodes[3])
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Polygon completely outside a plane.
|
||||
if ((Outcodes[0] & Outcodes[1] & Outcodes[2]) != 0)
|
||||
return false;
|
||||
|
||||
// If any vertex has an out code of all zeros then we intersect the volume.
|
||||
if (!Outcodes[0] || !Outcodes[1] || !Outcodes[2])
|
||||
return true;
|
||||
}
|
||||
|
||||
// Buffers for clipping the polygon.
|
||||
lcVector3 ClipPoints[2][8];
|
||||
int NumClipPoints[2];
|
||||
int ClipBuffer = 0;
|
||||
|
||||
NumClipPoints[0] = NumPoints;
|
||||
ClipPoints[0][0] = lcVector3(p1[0], p1[1], p1[2]);
|
||||
ClipPoints[0][1] = lcVector3(p2[0], p2[1], p2[2]);
|
||||
ClipPoints[0][2] = lcVector3(p3[0], p3[1], p3[2]);
|
||||
|
||||
if (NumPoints == 4)
|
||||
ClipPoints[0][3] = lcVector3(p4[0], p4[1], p4[2]);
|
||||
|
||||
// Now clip the polygon against the planes.
|
||||
for (i = 0; i < NumPlanes; i++)
|
||||
{
|
||||
lcPolygonPlaneClip(ClipPoints[ClipBuffer], NumClipPoints[ClipBuffer], ClipPoints[ClipBuffer^1], &NumClipPoints[ClipBuffer^1], Planes[i]);
|
||||
ClipBuffer ^= 1;
|
||||
|
||||
if (!NumClipPoints[ClipBuffer])
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename IndexType>
|
||||
bool lcMesh::IntersectsPlanes(const lcVector4 Planes[6])
|
||||
{
|
||||
|
|
@ -345,7 +231,7 @@ bool lcMesh::IntersectsPlanes(const lcVector4 Planes[6])
|
|||
IndexType* Indices = (IndexType*)mIndexBuffer.mData + Section->IndexOffset / sizeof(IndexType);
|
||||
|
||||
for (int Idx = 0; Idx < Section->NumIndices; Idx += 3)
|
||||
if (PolygonIntersectsPlanes(&Verts[Indices[Idx]*3], &Verts[Indices[Idx+1]*3], &Verts[Indices[Idx+2]*3], NULL, Planes))
|
||||
if (lcTriangleIntersectsPlanes(&Verts[Indices[Idx]*3], &Verts[Indices[Idx+1]*3], &Verts[Indices[Idx+2]*3], Planes))
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
#include "view.h"
|
||||
#include "library.h"
|
||||
#include "texfont.h"
|
||||
#include "algebra.h"
|
||||
#include "debug.h"
|
||||
#include "lc_application.h"
|
||||
|
||||
|
|
@ -6179,7 +6178,7 @@ void Project::GetPieceInsertPosition(View* view, int MouseX, int MouseY, lcVecto
|
|||
lcUnprojectPoints(ClickPoints, 2, ModelView, Projection, Viewport);
|
||||
|
||||
lcVector3 Intersection;
|
||||
if (lcLinePlaneIntersection(Intersection, ClickPoints[0], ClickPoints[1], lcVector4(0, 0, 1, 0)))
|
||||
if (lcLinePlaneIntersection(&Intersection, ClickPoints[0], ClickPoints[1], lcVector4(0, 0, 1, 0)))
|
||||
{
|
||||
SnapVector(Intersection);
|
||||
Position = Intersection;
|
||||
|
|
@ -6801,31 +6800,28 @@ bool Project::RotateSelectedObjects(lcVector3& Delta, lcVector3& Remainder)
|
|||
bs[2] = bs[5] = pos[2];
|
||||
}
|
||||
|
||||
Vector3 Center((bs[0]+bs[3])/2, (bs[1]+bs[4])/2, (bs[2]+bs[5])/2);
|
||||
lcVector3 Center((bs[0]+bs[3])/2, (bs[1]+bs[4])/2, (bs[2]+bs[5])/2);
|
||||
|
||||
// Create the rotation matrix.
|
||||
Quaternion Rotation(0, 0, 0, 1);
|
||||
Quaternion WorldToFocus, FocusToWorld;
|
||||
lcVector4 RotationQuaternion(0, 0, 0, 1);
|
||||
lcVector4 WorldToFocusQuaternion, FocusToWorldQuaternion;
|
||||
|
||||
if (!(m_nSnap & LC_DRAW_LOCK_X) && (Delta[0] != 0.0f))
|
||||
{
|
||||
Quaternion q;
|
||||
q.CreateRotationX(Delta[0] * LC_DTOR);
|
||||
Rotation = Mul(q, Rotation);
|
||||
lcVector4 q = lcQuaternionRotationX(Delta[0] * LC_DTOR);
|
||||
RotationQuaternion = lcQuaternionMultiply(q, RotationQuaternion);
|
||||
}
|
||||
|
||||
if (!(m_nSnap & LC_DRAW_LOCK_Y) && (Delta[1] != 0.0f))
|
||||
{
|
||||
Quaternion q;
|
||||
q.CreateRotationY(Delta[1] * LC_DTOR);
|
||||
Rotation = Mul(q, Rotation);
|
||||
lcVector4 q = lcQuaternionRotationY(Delta[1] * LC_DTOR);
|
||||
RotationQuaternion = lcQuaternionMultiply(q, RotationQuaternion);
|
||||
}
|
||||
|
||||
if (!(m_nSnap & LC_DRAW_LOCK_Z) && (Delta[2] != 0.0f))
|
||||
{
|
||||
Quaternion q;
|
||||
q.CreateRotationZ(Delta[2] * LC_DTOR);
|
||||
Rotation = Mul(q, Rotation);
|
||||
lcVector4 q = lcQuaternionRotationZ(Delta[2] * LC_DTOR);
|
||||
RotationQuaternion = lcQuaternionMultiply(q, RotationQuaternion);
|
||||
}
|
||||
|
||||
// Transform the rotation relative to the focused piece.
|
||||
|
|
@ -6837,10 +6833,10 @@ bool Project::RotateSelectedObjects(lcVector3& Delta, lcVector3& Remainder)
|
|||
lcVector4 Rot;
|
||||
Rot = ((Piece*)pFocus)->mRotation;
|
||||
|
||||
WorldToFocus.FromAxisAngle(Vector4(Rot[0], Rot[1], Rot[2], -Rot[3] * LC_DTOR));
|
||||
FocusToWorld.FromAxisAngle(Vector4(Rot[0], Rot[1], Rot[2], Rot[3] * LC_DTOR));
|
||||
WorldToFocusQuaternion = lcQuaternionFromAxisAngle(lcVector4(Rot[0], Rot[1], Rot[2], -Rot[3] * LC_DTOR));
|
||||
FocusToWorldQuaternion = lcQuaternionFromAxisAngle(lcVector4(Rot[0], Rot[1], Rot[2], Rot[3] * LC_DTOR));
|
||||
|
||||
Rotation = Mul(FocusToWorld, Rotation);
|
||||
RotationQuaternion = lcQuaternionMultiply(FocusToWorldQuaternion, RotationQuaternion);
|
||||
}
|
||||
|
||||
for (pPiece = m_pPieces; pPiece; pPiece = pPiece->m_pNext)
|
||||
|
|
@ -6851,55 +6847,53 @@ bool Project::RotateSelectedObjects(lcVector3& Delta, lcVector3& Remainder)
|
|||
pos = pPiece->mPosition;
|
||||
rot = pPiece->mRotation;
|
||||
|
||||
Vector4 NewRotation;
|
||||
lcVector4 NewRotation;
|
||||
|
||||
if ((nSel == 1) && (pFocus == pPiece))
|
||||
{
|
||||
Quaternion LocalToWorld;
|
||||
LocalToWorld.FromAxisAngle(Vector4(rot[0], rot[1], rot[2], rot[3] * LC_DTOR));
|
||||
lcVector4 LocalToWorldQuaternion;
|
||||
LocalToWorldQuaternion = lcQuaternionFromAxisAngle(lcVector4(rot[0], rot[1], rot[2], rot[3] * LC_DTOR));
|
||||
|
||||
Quaternion NewLocalToWorld;
|
||||
lcVector4 NewLocalToWorldQuaternion;
|
||||
|
||||
if (pFocus != NULL)
|
||||
{
|
||||
Quaternion LocalToFocus = Mul(WorldToFocus, LocalToWorld);
|
||||
NewLocalToWorld = Mul(LocalToFocus, Rotation);
|
||||
lcVector4 LocalToFocusQuaternion = lcQuaternionMultiply(WorldToFocusQuaternion, LocalToWorldQuaternion);
|
||||
NewLocalToWorldQuaternion = lcQuaternionMultiply(LocalToFocusQuaternion, RotationQuaternion);
|
||||
}
|
||||
else
|
||||
{
|
||||
NewLocalToWorld = Mul(Rotation, LocalToWorld);
|
||||
NewLocalToWorldQuaternion = lcQuaternionMultiply(RotationQuaternion, LocalToWorldQuaternion);
|
||||
}
|
||||
|
||||
NewLocalToWorld.ToAxisAngle(NewRotation);
|
||||
NewRotation = lcQuaternionToAxisAngle(NewLocalToWorldQuaternion);
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector3 Distance = Vector3(pos[0], pos[1], pos[2]) - Center;
|
||||
lcVector3 Distance = lcVector3(pos[0], pos[1], pos[2]) - Center;
|
||||
|
||||
Quaternion LocalToWorld;
|
||||
LocalToWorld.FromAxisAngle(Vector4(rot[0], rot[1], rot[2], rot[3] * LC_DTOR));
|
||||
lcVector4 LocalToWorldQuaternion = lcQuaternionFromAxisAngle(lcVector4(rot[0], rot[1], rot[2], rot[3] * LC_DTOR));
|
||||
|
||||
Quaternion NewLocalToWorld;
|
||||
lcVector4 NewLocalToWorldQuaternion;
|
||||
|
||||
if (pFocus != NULL)
|
||||
{
|
||||
Quaternion LocalToFocus = Mul(WorldToFocus, LocalToWorld);
|
||||
NewLocalToWorld = Mul(Rotation, LocalToFocus);
|
||||
lcVector4 LocalToFocusQuaternion = lcQuaternionMultiply(WorldToFocusQuaternion, LocalToWorldQuaternion);
|
||||
NewLocalToWorldQuaternion = lcQuaternionMultiply(RotationQuaternion, LocalToFocusQuaternion);
|
||||
|
||||
Quaternion WorldToLocal;
|
||||
WorldToLocal.FromAxisAngle(Vector4(rot[0], rot[1], rot[2], -rot[3] * LC_DTOR));
|
||||
lcVector4 WorldToLocalQuaternion = lcQuaternionFromAxisAngle(lcVector4(rot[0], rot[1], rot[2], -rot[3] * LC_DTOR));
|
||||
|
||||
Distance = Mul(Distance, WorldToLocal);
|
||||
Distance = Mul(Distance, NewLocalToWorld);
|
||||
Distance = lcQuaternionMul(Distance, WorldToLocalQuaternion);
|
||||
Distance = lcQuaternionMul(Distance, NewLocalToWorldQuaternion);
|
||||
}
|
||||
else
|
||||
{
|
||||
NewLocalToWorld = Mul(Rotation, LocalToWorld);
|
||||
NewLocalToWorldQuaternion = lcQuaternionMultiply(RotationQuaternion, LocalToWorldQuaternion);
|
||||
|
||||
Distance = Mul(Distance, Rotation);
|
||||
Distance = lcQuaternionMul(Distance, RotationQuaternion);
|
||||
}
|
||||
|
||||
NewLocalToWorld.ToAxisAngle(NewRotation);
|
||||
NewRotation = lcQuaternionToAxisAngle(NewLocalToWorldQuaternion);
|
||||
|
||||
pos[0] = Center[0] + Distance[0];
|
||||
pos[1] = Center[1] + Distance[1];
|
||||
|
|
@ -7222,15 +7216,15 @@ bool Project::OnKeyDown(char nKey, bool bControl, bool bShift)
|
|||
lcUnprojectPoints(Pts, 3, ModelView, Projection, Viewport);
|
||||
|
||||
float ax, ay;
|
||||
Vector3 vx((Pts[1][0] - Pts[0][0]), (Pts[1][1] - Pts[0][1]), 0);//Pts[1][2] - Pts[0][2] };
|
||||
lcVector3 vx((Pts[1][0] - Pts[0][0]), (Pts[1][1] - Pts[0][1]), 0);//Pts[1][2] - Pts[0][2] };
|
||||
vx.Normalize();
|
||||
Vector3 x(1, 0, 0);
|
||||
ax = acosf(Dot3(vx, x));
|
||||
lcVector3 x(1, 0, 0);
|
||||
ax = acosf(lcDot(vx, x));
|
||||
|
||||
Vector3 vy((Pts[2][0] - Pts[0][0]), (Pts[2][1] - Pts[0][1]), 0);//Pts[2][2] - Pts[0][2] };
|
||||
lcVector3 vy((Pts[2][0] - Pts[0][0]), (Pts[2][1] - Pts[0][1]), 0);//Pts[2][2] - Pts[0][2] };
|
||||
vy.Normalize();
|
||||
Vector3 y(0, -1, 0);
|
||||
ay = acosf(Dot3(vy, y));
|
||||
lcVector3 y(0, -1, 0);
|
||||
ay = acosf(lcDot(vy, y));
|
||||
|
||||
if (ax > 135)
|
||||
axis[0] = -axis[0];
|
||||
|
|
@ -7242,7 +7236,7 @@ bool Project::OnKeyDown(char nKey, bool bControl, bool bShift)
|
|||
{
|
||||
float tmp = axis[0];
|
||||
|
||||
ax = acosf(Dot3(vx, y));
|
||||
ax = acosf(lcDot(vx, y));
|
||||
if (ax > 90)
|
||||
{
|
||||
axis[0] = -axis[1];
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
#include "typedefs.h"
|
||||
#include "opengl.h"
|
||||
#include "array.h"
|
||||
#include "algebra.h"
|
||||
#include "lc_math.h"
|
||||
|
||||
typedef enum
|
||||
|
|
|
|||
|
|
@ -161,7 +161,6 @@
|
|||
<ClCompile Include="Tools.cpp" />
|
||||
<ClCompile Include="transdlg.cpp" />
|
||||
<ClCompile Include="win_gl.cpp" />
|
||||
<ClCompile Include="..\common\algebra.cpp" />
|
||||
<ClCompile Include="..\Common\camera.cpp" />
|
||||
<ClCompile Include="..\common\console.cpp" />
|
||||
<ClCompile Include="..\common\curve.cpp" />
|
||||
|
|
|
|||
|
|
@ -170,9 +170,6 @@
|
|||
<ClCompile Include="win_gl.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\common\algebra.cpp">
|
||||
<Filter>Common Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\Common\camera.cpp">
|
||||
<Filter>Common Source Files</Filter>
|
||||
</ClCompile>
|
||||
|
|
|
|||
Loading…
Reference in a new issue