Show kquaternion.cpp syntax highlighted
/*
Copyright (C) 2003, 2004, 2005 by Luca Cappa
Written by Luca Cappa groton@users.sourceforge.net
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Library General Public
License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Library General Public License for more details.
You should have received a copy of the GNU Library General Public
License along with this library; if not, write to the Free
Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "cssysdef.h"
#include "csutil/sysfunc.h"
#include "csutil/cscolor.h"
#include "csutil/cmdhelp.h"
#include "csutil/cspmeter.h"
#include "csutil/csstring.h"
#include "csutil/scfstr.h"
#include "cstool/csview.h"
#include "cstool/initapp.h"
#include "cstool/collider.h"
#include "iutil/vfs.h"
#include "iutil/eventq.h"
#include "iutil/event.h"
#include "iutil/objreg.h"
#include "iutil/csinput.h"
#include "iutil/virtclk.h"
#include "iutil/plugin.h"
#include "iutil/string.h"
#include "iengine/sector.h"
#include "iengine/engine.h"
#include "iengine/camera.h"
#include "iengine/light.h"
#include "iengine/texture.h"
#include "iengine/mesh.h"
#include "iengine/movable.h"
#include "iengine/material.h"
#include "imesh/thing.h"
#include "imesh/object.h"
#include "imesh/sprite3d.h"
#include "ivideo/graph3d.h"
#include "ivideo/graph2d.h"
#include "ivideo/natwin.h"
#include "ivideo/txtmgr.h"
#include "ivideo/texture.h"
#include "ivideo/material.h"
#include "ivideo/fontserv.h"
#include "igraphic/imageio.h"
#include "imap/loader.h"
#include "ivaria/reporter.h"
#include "ivaria/stdrep.h"
#include "ivaria/conout.h"
#include "ivaria/reporter.h"
#include "ivaria/stdrep.h"
#include "ivaria/collider.h"
#include "csgeom/quaterni.h"
#include "csgeom/transfrm.h"
#include "csgeom/math3d_d.h"
#include "csgeom/math3d.h"
#include "igeom/polymesh.h"
#include "igeom/objmodel.h"
#include "imap/loader.h"
#include "iaws/aws.h"
#include "iaws/awscnvs.h"
#include "korientation.h"
#include "kblock.h"
#include "kmap.h"
#include "kblockid.h"
#include "kflags.h"
#include "kappstate.h"
#include "ske.h"
#include "kdata.h"
#include "kquaternion.h"
void KQuaternion::SetWithEuler (const csVector3& p_angles)
{
EulerAngles l_eA = {p_angles.y, p_angles.x, p_angles.z,
m_order};
Quat q = Eul_ToQuat (l_eA);
x = q.x; y = q.y; z = q.z; r = q.w;
}
void KQuaternion::SetWithEuler (const csVector3& p_angles, int p_order)
{
EulerAngles l_eA = {p_angles.y, p_angles.x, p_angles.z,
p_order};
Quat q = Eul_ToQuat (l_eA);
x = q.x; y = q.y; z = q.z; r = q.w;
}
void KQuaternion::GetEulerAngles (csVector3& p_angles)
{
Quat q;
q.x = x; q.y = y; q.z = z; q.w = r;
EulerAngles l_eA = Eul_FromQuat (q, m_order);
p_angles.x = l_eA.y;
p_angles.y = l_eA.x;
p_angles.z = l_eA.z;
}
void KQuaternion::GetEulerAngles (csVector3& p_angles, int p_order)
{
Quat q;
q.x = x; q.y = y; q.z = z; q.w = r;
EulerAngles l_eA = Eul_FromQuat (q, p_order);
p_angles.x = l_eA.y;
p_angles.y = l_eA.x;
p_angles.z = l_eA.z;
}
void KQuaternion::Invert()
{
float norm, invNorm;
norm = x * x + y * y + z * z + r * r;
invNorm = (float) (1.0 / norm);
x = -x * invNorm;
y = -y * invNorm;
z = -z * invNorm;
r = r * invNorm;
}
KQuaternion::KQuaternion(const csMatrix3& mat, int p_order)
{
m_order = p_order;
float tr, s;
int i;
tr = mat.m11 + mat.m22 + mat.m33 + 1.0;
// check the diagonal
if (tr > 0.0) {
s = 0.5 / sqrt (tr);
r = 0.25 / s;
x = (mat.m32 - mat.m23) * s;
y = (mat.m13 - mat.m31) * s;
z = (mat.m21 - mat.m12) * s;
} else {
// diagonal is negative
i = 1;
if (mat.m22 > mat.m11) i = 2;
if ((i == 1 && mat.m33 > mat.m11)
|| (i == 2 && mat.m33 > mat.m22)) i = 3;
/*
m11 = 0
m12 = 1
m13 = 2
m21 = 4
m22 = 5
m23 = 6
m31 = 8
m32 = 9
m33 = 10
*/
switch(i) {
case 1:
s = sqrt ((mat.m11 - (mat.m22 + mat.m33)) + 1.0);
x = s * 0.5;
if (s != 0.0) s = 0.5 / s;
y = (mat.m12 + mat.m21) * s;
z = (mat.m13 + mat.m31) * s;
r = (mat.m23 - mat.m32) * s;
break;
case 2:
s = sqrt ((mat.m22 - (mat.m33 + mat.m11)) + 1.0);
y = 0.5 * s;
if (s != 0.0) s = 0.5 / s;
x = (mat.m12 + mat.m21) * s;
z = (mat.m23 + mat.m32) * s;
r = (mat.m13 - mat.m31) * s;
break;
case 3:
s = sqrt ((mat.m33 - (mat.m11 + mat.m22)) + 1.0);
z = 0.5 * s;
if (s != 0.0) s = 0.5 / s;
x = (mat.m13 + mat.m31) * s;
y = (mat.m23 + mat.m32) * s;
r = (mat.m12 - mat.m21) * s;
break;
}
}
}
#define SLERPDELTA 0.0001f \
\
// Difference at which to lerp instead of slerp
KQuaternion KQuaternion::Slerp (const KQuaternion &quat2, float slerp) const
{
double omega, cosom, invsinom, scale0, scale1;
KQuaternion quato(quat2);
// decide if one of the quaternions is backwards
double a = (x-quat2.x)*(x-quat2.x) + (y-quat2.y)*(y-quat2.y) + (z-quat2.z)*(z-quat2.z) + (r-quat2.r)*(r-quat2.r);
double b = (x+quat2.x)*(x+quat2.x) + (y+quat2.y)*(y+quat2.y) + (z+quat2.z)*(z+quat2.z) + (r+quat2.r)*(r+quat2.r);
if (a > b)
{
quato.Negate();
}
// Calculate dot between quats
cosom = x * quato.x + y * quato.y + z * quato.z + r * quato.r;
// Make sure the two quaternions are not exactly opposite? (within a little
// slop).
if ((1.0f + cosom) > SLERPDELTA)
{
// Are they more than a little bit different? Avoid a divided by zero
// and lerp if not.
if ((1.0f - cosom) > SLERPDELTA)
{
// Yes, do a slerp
omega = acos (cosom);
invsinom = 1.0f / sin (omega);
scale0 = sin ((1.0f - slerp) * omega) * invsinom;
scale1 = sin (slerp * omega) * invsinom;
}
else
{
// Not a very big difference, do a lerp
scale0 = 1.0f - slerp;
scale1 = slerp;
}
return KQuaternion (
scale0 * r + scale1 * quato.r,
scale0 * x + scale1 * quato.x,
scale0 * y + scale1 * quato.y,
scale0 * z + scale1 * quato.z);
}
// The quaternions are nearly opposite so to avoid a divided by zero error
// Calculate a perpendicular quaternion and slerp that direction
scale0 = sin ((1.0f - slerp) * PI);
scale1 = sin (slerp * PI);
return KQuaternion (
scale0 * r + scale1 * quato.z,
scale0 * x + scale1 * -quato.y,
scale0 * y + scale1 * quato.x,
scale0 * z + scale1 * -quato.r);
}
const csMatrix3 KQuaternion::GetMatrix () const
{
float rx, ry, rz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
csMatrix3 m;
// calculate coefficients
x2 = x + x;
y2 = y + y;
z2 = z + z;
xx = x * x2; xy = x * y2; xz = x * z2;
yy = y * y2; yz = y * z2; zz = z * z2;
rx = r * x2; ry = r * y2; rz = r * z2;
m.m11 = 1.0 - (yy + zz);
m.m12 = xy - rz;
m.m13 = xz + ry;
m.m21 = xy + rz;
m.m22 = 1.0 - (xx + zz);
m.m23 = yz - rx;
m.m31 = xz - ry;
m.m32 = yz + rx;
m.m33 = 1.0 - (xx + yy);
return m;
}
See more files for this project here