You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
443 lines
11 KiB
443 lines
11 KiB
4 years ago
|
/*==LICENSE==*
|
||
|
|
|
||
|
|
CyanWorlds.com Engine - MMOG client, server and tools
|
||
|
|
Copyright (C) 2011 Cyan Worlds, Inc.
|
||
|
|
|
||
|
|
This program is free software: you can redistribute it and/or modify
|
||
|
|
it under the terms of the GNU General Public License as published by
|
||
|
|
the Free Software Foundation, either version 3 of the License, or
|
||
|
|
(at your option) any later version.
|
||
|
|
|
||
|
|
This program 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 General Public License for more details.
|
||
|
|
|
||
|
|
You should have received a copy of the GNU General Public License
|
||
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
|
||
|
|
Additional permissions under GNU GPL version 3 section 7
|
||
|
|
|
||
|
|
If you modify this Program, or any covered work, by linking or
|
||
|
|
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
|
||
|
|
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
|
||
|
|
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
|
||
|
|
(or a modified version of those libraries),
|
||
|
|
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
|
||
|
|
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
|
||
|
|
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
|
||
|
|
licensors of this Program grant you additional
|
||
|
|
permission to convey the resulting work. Corresponding Source for a
|
||
|
|
non-source form of such a combination shall include the source code for
|
||
|
|
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
|
||
|
|
work.
|
||
|
|
|
||
|
|
You can contact Cyan Worlds, Inc. by email legal@cyan.com
|
||
|
|
or by snail mail at:
|
||
|
|
Cyan Worlds, Inc.
|
||
|
|
14617 N Newport Hwy
|
||
|
|
Mead, WA 99021
|
||
|
|
|
||
|
|
*==LICENSE==*/
|
||
|
|
#include "HeadSpin.h"
|
||
|
|
#include "hsAffineParts.h"
|
||
|
|
#include "../plInterp/hsInterp.h"
|
||
|
|
#include "hsStream.h"
|
||
|
|
|
||
|
|
#include "plProfile.h"
|
||
|
|
|
||
|
|
#define PL_OPTIMIZE_COMPOSE
|
||
|
|
|
||
|
|
inline void QuatTo3Vectors(const hsQuat& q, hsVector3* const v)
|
||
|
|
{
|
||
|
|
v[0][0] = 1.0f - 2.0f*q.fY*q.fY - 2.0f*q.fZ*q.fZ;
|
||
|
|
v[0][1] = 2.0f*q.fX*q.fY - 2.0f*q.fW*q.fZ;
|
||
|
|
v[0][2] = 2.0f*q.fX*q.fZ + 2.0f*q.fW*q.fY;
|
||
|
|
|
||
|
|
v[1][0] = 2.0f*q.fX*q.fY + 2.0f*q.fW*q.fZ;
|
||
|
|
v[1][1] = 1.0f - 2.0f*q.fX*q.fX - 2.0f*q.fZ*q.fZ;
|
||
|
|
v[1][2] = 2.0f*q.fY*q.fZ - 2.0f*q.fW*q.fX;
|
||
|
|
|
||
|
|
v[2][0] = 2.0f*q.fX*q.fZ - 2.0f*q.fW*q.fY;
|
||
|
|
v[2][1] = 2.0f*q.fY*q.fZ + 2.0f*q.fW*q.fX;
|
||
|
|
v[2][2] = 1.0f - 2.0f*q.fX*q.fX - 2.0f*q.fY*q.fY;
|
||
|
|
}
|
||
|
|
|
||
|
|
inline void QuatTo3VectorsTranspose(const hsQuat& q, hsVector3* const v)
|
||
|
|
{
|
||
|
|
v[0][0] = 1.0f - 2.0f*q.fY*q.fY - 2.0f*q.fZ*q.fZ;
|
||
|
|
v[1][0] = 2.0f*q.fX*q.fY - 2.0f*q.fW*q.fZ;
|
||
|
|
v[2][0] = 2.0f*q.fX*q.fZ + 2.0f*q.fW*q.fY;
|
||
|
|
|
||
|
|
v[0][1] = 2.0f*q.fX*q.fY + 2.0f*q.fW*q.fZ;
|
||
|
|
v[1][1] = 1.0f - 2.0f*q.fX*q.fX - 2.0f*q.fZ*q.fZ;
|
||
|
|
v[2][1] = 2.0f*q.fY*q.fZ - 2.0f*q.fW*q.fX;
|
||
|
|
|
||
|
|
v[0][2] = 2.0f*q.fX*q.fZ - 2.0f*q.fW*q.fY;
|
||
|
|
v[1][2] = 2.0f*q.fY*q.fZ + 2.0f*q.fW*q.fX;
|
||
|
|
v[2][2] = 1.0f - 2.0f*q.fX*q.fX - 2.0f*q.fY*q.fY;
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
// Constructors
|
||
|
|
// Convert from Gems struct for now
|
||
|
|
//
|
||
|
|
hsAffineParts::hsAffineParts(gemAffineParts *ap)
|
||
|
|
{
|
||
|
|
AP_SET((*this), (*ap));
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
//
|
||
|
|
//
|
||
|
|
hsAffineParts::hsAffineParts()
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
//
|
||
|
|
//
|
||
|
|
void hsAffineParts::Reset()
|
||
|
|
{
|
||
|
|
fT.Set(0,0,0);
|
||
|
|
fQ.Identity();
|
||
|
|
fU.Identity();
|
||
|
|
fK.Set(1,1,1);
|
||
|
|
fF = 1.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
plProfile_CreateTimer("Compose", "Affine", Compose);
|
||
|
|
plProfile_CreateTimer("ComposeInv", "Affine", ComposeInv);
|
||
|
|
//
|
||
|
|
// Create an affine matrix from the various parts
|
||
|
|
//
|
||
|
|
// AffineParts:
|
||
|
|
// Vector t; /* Translation components */
|
||
|
|
// Quat q; /* Essential rotation */
|
||
|
|
// Quat u; /* Stretch rotation */
|
||
|
|
// Vector k; /* Stretch factors */
|
||
|
|
// float f; /* Sign of determinant */
|
||
|
|
//
|
||
|
|
// A matrix M is decomposed by : M = T F R U K Utranspose.
|
||
|
|
// T is the translate mat.
|
||
|
|
// F is +-Identity (to flip the rotation or not).
|
||
|
|
// R is the rot matrix.
|
||
|
|
// U is the stretch matrix.
|
||
|
|
// K is the scale factor matrix.
|
||
|
|
//
|
||
|
|
void hsAffineParts::ComposeMatrix(hsMatrix44 *out) const
|
||
|
|
{
|
||
|
|
plProfile_BeginTiming(Compose);
|
||
|
|
#ifndef PL_OPTIMIZE_COMPOSE
|
||
|
|
// Built U matrix
|
||
|
|
hsMatrix44 U;
|
||
|
|
fU.MakeMatrix(&U);
|
||
|
|
|
||
|
|
// Build scale factor matrix
|
||
|
|
hsMatrix44 K;
|
||
|
|
K.MakeScaleMat(&fK);
|
||
|
|
|
||
|
|
// Build Utranspose matrix
|
||
|
|
hsMatrix44 Utp;
|
||
|
|
U.GetTranspose(&Utp);
|
||
|
|
|
||
|
|
// Build R matrix
|
||
|
|
hsMatrix44 R;
|
||
|
|
fQ.MakeMatrix(&R);
|
||
|
|
|
||
|
|
// Build flip matrix
|
||
|
|
// hsAssert(fF == 1.0 || fF == -1.0, "Invalid flip portion of affine parts");
|
||
|
|
hsMatrix44 F;
|
||
|
|
if (fF==-1.0)
|
||
|
|
{
|
||
|
|
hsVector3 s;
|
||
|
|
s.Set(-1,-1,-1);
|
||
|
|
F.MakeScaleMat(&s);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
F.Reset();
|
||
|
|
|
||
|
|
// Build translate matrix
|
||
|
|
hsMatrix44 T;
|
||
|
|
T.MakeTranslateMat(&fT);
|
||
|
|
|
||
|
|
//
|
||
|
|
// Concat mats
|
||
|
|
//
|
||
|
|
*out = K * Utp;
|
||
|
|
*out = U * (*out);
|
||
|
|
*out = R * (*out); // Q
|
||
|
|
*out = F * (*out);
|
||
|
|
*out = T * (*out); // Translate happens last
|
||
|
|
#else // PL_OPTIMIZE_COMPOSE
|
||
|
|
// M = T F R U K Ut,
|
||
|
|
// but these are mostly very sparse matrices. So rather
|
||
|
|
// than construct the full 6 matrices and concatenate them,
|
||
|
|
// we'll work out by hand what the non-zero results will be.
|
||
|
|
// T = |1 0 0 Tx|
|
||
|
|
// |0 1 0 Ty|
|
||
|
|
// |0 0 1 Tz|
|
||
|
|
// F = |f 0 0 0|
|
||
|
|
// |0 f 0 0|
|
||
|
|
// |0 0 f 0|, where f is either 1 or -1
|
||
|
|
// R = |R00 R01 R02 0|
|
||
|
|
// |R10 R11 R12 0|
|
||
|
|
// |R20 R21 R22 0|
|
||
|
|
// U = |U00 U01 U02 0|
|
||
|
|
// |U10 U11 U12 0|
|
||
|
|
// |U20 U21 U22 0|
|
||
|
|
// K = |Sx 0 0 0|
|
||
|
|
// |0 Sy 0 0|
|
||
|
|
// |0 0 Sz 0|
|
||
|
|
// Ut = |U00 U10 U20 0|
|
||
|
|
// |U01 U11 U21 0|
|
||
|
|
// |U02 U12 U22 0|, where Uij is from matrix U
|
||
|
|
//
|
||
|
|
// So, K * Ut =
|
||
|
|
// |Sx*U00 Sx*U10 Sx*U20 0|
|
||
|
|
// |Sy*U01 Sy*U11 Sy*U21 0|
|
||
|
|
// |Sz*U02 Sz*U12 Sz*U22 0|
|
||
|
|
//
|
||
|
|
// U * (K * Ut) =
|
||
|
|
// | U0 dot S*U0 U0 dot S*U1 U0 dot S*U2 0|
|
||
|
|
// | U1 dot S*U0 U1 dot S*U1 U1 dot S*U2 0|
|
||
|
|
// | U2 dot S*U0 U2 dot S*U1 U2 dot S*U2 0|
|
||
|
|
//
|
||
|
|
// Let's call that matrix UK
|
||
|
|
//
|
||
|
|
// Now R * U * K * Ut = R * UK =
|
||
|
|
// | R0 dot UKc0 R0 dot UKc1 R0 dot UKc2 0|
|
||
|
|
// | R1 dot UKc0 R1 dot UKc1 R1 dot UKc2 0|
|
||
|
|
// | R2 dot UKc0 R2 dot UKc1 R2 dot UKc2 0|, where UKci is column i from UK
|
||
|
|
//
|
||
|
|
// if f is -1, we negate the matrix we have so far, else we don't. We can
|
||
|
|
// accomplish this cleanly by just negating the scale vector S if f == -1.
|
||
|
|
//
|
||
|
|
// Since the translate is last, we can just stuff it into the 4th column.
|
||
|
|
//
|
||
|
|
// Since we only ever use UK as column vectors, we'll just construct it
|
||
|
|
// into 3 vectors representing the columns.
|
||
|
|
//
|
||
|
|
// The quat MakeMatrix function is pretty efficient, but it does a little more work
|
||
|
|
// than it has to filling out the whole matrix when we only need the 3x3 rotation,
|
||
|
|
// and we'd rather have it in the form of vectors anyway, so we'll use our own
|
||
|
|
// quat to 3 vectors function here.
|
||
|
|
|
||
|
|
hsVector3 U[3];
|
||
|
|
QuatTo3Vectors(fU, U);
|
||
|
|
|
||
|
|
int i, j;
|
||
|
|
|
||
|
|
hsVector3 UKt[3];
|
||
|
|
for( i = 0; i < 3; i++ )
|
||
|
|
{
|
||
|
|
for( j = 0; j < 3; j++ )
|
||
|
|
{
|
||
|
|
// SU[j] = (fK.fX * U[j].fX, fK.fY * U[j].fY, fK.fZ * U[j].fZ)
|
||
|
|
UKt[j][i] = U[i].fX * fK.fX * U[j].fX
|
||
|
|
+ U[i].fY * fK.fY * U[j].fY
|
||
|
|
+ U[i].fZ * fK.fZ * U[j].fZ;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
hsVector3 R[3];
|
||
|
|
QuatTo3Vectors(fQ, R);
|
||
|
|
|
||
|
|
hsScalar f = fF < 0 ? -1.f : 1.f;
|
||
|
|
for( i = 0; i < 3; i++ )
|
||
|
|
{
|
||
|
|
for( j = 0; j < 3; j++ )
|
||
|
|
{
|
||
|
|
out->fMap[i][j] = R[i].InnerProduct(UKt[j]) * f;
|
||
|
|
}
|
||
|
|
|
||
|
|
out->fMap[i][3] = fT[i];
|
||
|
|
}
|
||
|
|
|
||
|
|
out->fMap[3][0] = out->fMap[3][1] = out->fMap[3][2] = 0.f;
|
||
|
|
out->fMap[3][3] = 1.f;
|
||
|
|
out->NotIdentity();
|
||
|
|
|
||
|
|
#endif // PL_OPTIMIZE_COMPOSE
|
||
|
|
plProfile_EndTiming(Compose);
|
||
|
|
}
|
||
|
|
|
||
|
|
void hsAffineParts::ComposeInverseMatrix(hsMatrix44 *out) const
|
||
|
|
{
|
||
|
|
plProfile_BeginTiming(Compose);
|
||
|
|
#ifndef PL_OPTIMIZE_COMPOSE
|
||
|
|
// Built U matrix
|
||
|
|
hsMatrix44 U;
|
||
|
|
fU.Conjugate().MakeMatrix(&U);
|
||
|
|
|
||
|
|
// Build scale factor matrix
|
||
|
|
hsMatrix44 K;
|
||
|
|
hsVector3 invK;
|
||
|
|
invK.Set(hsScalarInvert(fK.fX),hsScalarInvert(fK.fY),hsScalarInvert(fK.fZ));
|
||
|
|
K.MakeScaleMat(&invK);
|
||
|
|
|
||
|
|
// Build Utranspose matrix
|
||
|
|
hsMatrix44 Utp;
|
||
|
|
U.GetTranspose(&Utp);
|
||
|
|
|
||
|
|
// Build R matrix
|
||
|
|
hsMatrix44 R;
|
||
|
|
fQ.Conjugate().MakeMatrix(&R);
|
||
|
|
|
||
|
|
// Build flip matrix
|
||
|
|
// hsAssert(fF == 1.0 || fF == -1.0, "Invalid flip portion of affine parts");
|
||
|
|
hsMatrix44 F;
|
||
|
|
if (fF==-1.0)
|
||
|
|
{
|
||
|
|
hsVector3 s;
|
||
|
|
s.Set(-1,-1,-1);
|
||
|
|
F.MakeScaleMat(&s);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
F.Reset();
|
||
|
|
|
||
|
|
// Build translate matrix
|
||
|
|
hsMatrix44 T;
|
||
|
|
T.MakeTranslateMat(&-fT);
|
||
|
|
|
||
|
|
//
|
||
|
|
// Concat mats
|
||
|
|
//
|
||
|
|
*out = Utp * K;
|
||
|
|
*out = (*out) * U;
|
||
|
|
*out = (*out) * R;
|
||
|
|
*out = (*out) * F;
|
||
|
|
*out = (*out) * T;
|
||
|
|
#else // PL_OPTIMIZE_COMPOSE
|
||
|
|
// Same kind of thing here, except now M = Ut K U R F T
|
||
|
|
// and again
|
||
|
|
// T = |1 0 0 Tx|
|
||
|
|
// |0 1 0 Ty|
|
||
|
|
// |0 0 1 Tz|
|
||
|
|
// F = |f 0 0 0|
|
||
|
|
// |0 f 0 0|
|
||
|
|
// |0 0 f 0|, where f is either 1 or -1
|
||
|
|
// R = |R00 R01 R02 0|
|
||
|
|
// |R10 R11 R12 0|
|
||
|
|
// |R20 R21 R22 0|
|
||
|
|
// U = |U00 U01 U02 0|
|
||
|
|
// |U10 U11 U12 0|
|
||
|
|
// |U20 U21 U22 0|
|
||
|
|
// K = |Sx 0 0 0|
|
||
|
|
// |0 Sy 0 0|
|
||
|
|
// |0 0 Sz 0|
|
||
|
|
// Ut = |U00 U10 U20 0|
|
||
|
|
// |U01 U11 U21 0|
|
||
|
|
// |U02 U12 U22 0|, where Uij is from matrix U
|
||
|
|
//
|
||
|
|
// So, Ut * K =
|
||
|
|
// |U00*Sx U10*Sy U20*Sz 0|
|
||
|
|
// |U01*Sx U11*Sy U21*Sz 0|
|
||
|
|
// |U02*Sx U12*Sy U22*Sz 0|
|
||
|
|
//
|
||
|
|
// (Ut * K) * U = UK =
|
||
|
|
// |Ut0*S dot Ut0 Ut0*S dot Ut1 Ut0*S dot Ut2 0|
|
||
|
|
// |Ut1*S dot Ut0 Ut1*S dot Ut1 Ut1*S dot Ut2 0|
|
||
|
|
// |Ut2*S dot Ut0 Ut2*S dot Ut1 Ut2*S dot Ut2 0|
|
||
|
|
//
|
||
|
|
// (((Ut * K) * U) * R)[i][j] = UK[i] dot Rc[j]
|
||
|
|
//
|
||
|
|
// Again we'll stuff the flip into the scale.
|
||
|
|
//
|
||
|
|
// Now, because the T is on the other end of the concat (closest
|
||
|
|
// to the vertex), we can't just stuff it in. If Mr is the
|
||
|
|
// rotation part of the final matrix (Ut * K * U * R * F), then
|
||
|
|
// the translation components M[i][3] = Mr[i] dot T.
|
||
|
|
//
|
||
|
|
//
|
||
|
|
hsVector3 Ut[3];
|
||
|
|
QuatTo3VectorsTranspose(fU.Conjugate(), Ut);
|
||
|
|
|
||
|
|
int i, j;
|
||
|
|
|
||
|
|
hsVector3 invK;
|
||
|
|
invK.Set(hsScalarInvert(fK.fX),hsScalarInvert(fK.fY),hsScalarInvert(fK.fZ));
|
||
|
|
hsVector3 UK[3];
|
||
|
|
for( i = 0; i < 3; i++ )
|
||
|
|
{
|
||
|
|
for( j = 0; j < 3; j++ )
|
||
|
|
{
|
||
|
|
// SUt[i] = (Ut[i].fX * invK.fX, Ut[i].fY * invK.fY, Ut[i].fZ * invK.fZ)
|
||
|
|
// So SUt[i].InnerProduct(Ut[j]) ==
|
||
|
|
// Ut[i].fX * invK.fX * Ut[j].fX
|
||
|
|
// + Ut[i].fY * invK.fY * Ut[j].fY
|
||
|
|
// + Ut[i].fZ * invK.fZ * Ut[j].fZ
|
||
|
|
|
||
|
|
UK[i][j] = Ut[i].fX * invK.fX * Ut[j].fX
|
||
|
|
+ Ut[i].fY * invK.fY * Ut[j].fY
|
||
|
|
+ Ut[i].fZ * invK.fZ * Ut[j].fZ;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
hsVector3 Rt[3];
|
||
|
|
QuatTo3VectorsTranspose(fQ.Conjugate(), Rt);
|
||
|
|
|
||
|
|
hsScalar f = fF < 0 ? -1.f : 1.f;
|
||
|
|
for( i = 0; i < 3; i++ )
|
||
|
|
{
|
||
|
|
for( j = 0; j < 3; j++ )
|
||
|
|
{
|
||
|
|
out->fMap[i][j] = UK[i].InnerProduct(Rt[j]) * f;
|
||
|
|
}
|
||
|
|
|
||
|
|
out->fMap[i][3] = -(fT.InnerProduct((hsPoint3*)(&out->fMap[i])));
|
||
|
|
}
|
||
|
|
|
||
|
|
out->fMap[3][0] = out->fMap[3][1] = out->fMap[3][2] = 0.f;
|
||
|
|
out->fMap[3][3] = 1.f;
|
||
|
|
out->NotIdentity();
|
||
|
|
|
||
|
|
#endif // PL_OPTIMIZE_COMPOSE
|
||
|
|
plProfile_EndTiming(Compose);
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
// Given 2 affineparts structs and a p value (between 0-1),
|
||
|
|
// compute a new affine parts.
|
||
|
|
//
|
||
|
|
void hsAffineParts::SetFromInterp(const hsAffineParts &ap1, const hsAffineParts &ap2, float p)
|
||
|
|
{
|
||
|
|
hsAssert(p>=0.0 && p<=1.0, "Interpolate param must be 0-1");
|
||
|
|
|
||
|
|
#if 0
|
||
|
|
// Debug
|
||
|
|
float rad1,rad2, rad3;
|
||
|
|
hsVector3 axis1, axis2, axis3;
|
||
|
|
k1->fQ.GetAngleAxis(&rad1, &axis1);
|
||
|
|
k2->fQ.GetAngleAxis(&rad2, &axis2);
|
||
|
|
fQ.GetAngleAxis(&rad3, &axis3);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
hsInterp::LinInterp(&ap1, &ap2, p, this);
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
// Read
|
||
|
|
//
|
||
|
|
void hsAffineParts::Read(hsStream *stream)
|
||
|
|
{
|
||
|
|
fT.Read(stream);
|
||
|
|
fQ.Read(stream);
|
||
|
|
fU.Read(stream);
|
||
|
|
fK.Read(stream);
|
||
|
|
fF = stream->ReadSwapFloat();
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
// Write
|
||
|
|
//
|
||
|
|
void hsAffineParts::Write(hsStream *stream)
|
||
|
|
{
|
||
|
|
fT.Write(stream);
|
||
|
|
fQ.Write(stream);
|
||
|
|
fU.Write(stream);
|
||
|
|
fK.Write(stream);
|
||
|
|
stream->WriteSwapFloat(fF);
|
||
|
|
}
|