CWE-ou-minkata/MOULOpenSourceClientPlugin/Plasma20/Sources/Plasma/PubUtilLib/plTransform/hsAffineParts.cpp

/*==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/>.

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);
}