//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#ifndef DY_FEATHERSTONE_ARTICULATION_UTIL_H
#define DY_FEATHERSTONE_ARTICULATION_UTIL_H

#include "PsVecMath.h"
#include "CmSpatialVector.h"
#include "PsBitUtils.h"
#include "foundation/PxMemory.h"

namespace physx
{

namespace Dy
{
	static const size_t DY_MAX_DOF = 6;

	struct SpatialSubspaceMatrix
	{
		static const PxU32 MaxColumns = 3;
	public:

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialSubspaceMatrix() :numColumns(0)
		{
			//PxMemZero(columns, sizeof(Cm::SpatialVectorF) * 6);
			memset(columns, 0, sizeof(Cm::UnAlignedSpatialVector) * MaxColumns);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE void setNumColumns(const PxU32 nc)
		{
			numColumns = nc;
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 getNumColumns() const
		{
			return numColumns;
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF transposeMultiply(Cm::SpatialVectorF& v) const
		{
			PxReal result[6];
			for (PxU32 i = 0; i < numColumns; ++i)
			{
				const Cm::UnAlignedSpatialVector& row = columns[i];
				result[i] = row.dot(v);
			}

			Cm::SpatialVectorF res;
			res.top.x = result[0]; res.top.y = result[1]; res.top.z = result[2];
			res.bottom.x = result[3]; res.bottom.y = result[4]; res.bottom.z = result[5];

			return res;

		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE void setColumn(const PxU32 index, const PxVec3& top, const PxVec3& bottom)
		{
			PX_ASSERT(index < MaxColumns);
			columns[index] = Cm::SpatialVectorF(top, bottom);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector& operator[](unsigned int num)
		{
			PX_ASSERT(num < MaxColumns);
			return columns[num];
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& operator[](unsigned int num)  const
		{
			PX_ASSERT(num < MaxColumns);
			return columns[num];
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE const Cm::UnAlignedSpatialVector* getColumns() const
		{
			return columns;
		}


	private:
		Cm::UnAlignedSpatialVector columns[MaxColumns];			//192		192			
		PxU32	numColumns;						//4			208 (12 bytes padding)

	};

	//this should be 6x6 matrix
	//|R,		0|
	//|-R*rX,	R|
	struct SpatialTransform
	{
		PxMat33 R;
		PxQuat q;
		PxMat33 T;

	public:
		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform() : R(PxZero), T(PxZero)
		{
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform(const PxMat33& R_, const PxMat33& T_) : R(R_), T(T_)
		{
			q = PxQuat(R_);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform(const PxQuat& q_, const PxMat33& T_) : q(q_), T(T_)
		{
			R = PxMat33(q_);
		}

		//This assume angular is the top vector and linear is the bottom vector
		/*PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVector operator *(const Cm::SpatialVector& s) const
		{
			const PxVec3 angular = R * s.angular;
			const PxVec3 linear = T * s.angular + R * s.linear;
			return Cm::SpatialVector(linear, angular);
		}*/


		////This assume angular is the top vector and linear is the bottom vector
		//PX_FORCE_INLINE Cm::SpatialVectorF operator *(Cm::SpatialVectorF& s) const
		//{
		//	const PxVec3 top = R * s.top;
		//	const PxVec3 bottom = T * s.top + R * s.bottom;

		//	const PxVec3 top1 = q.rotate(s.top);
		//	const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);

		///*	const PxVec3 tDif = (top - top1).abs();
		//	const PxVec3 bDif = (bottom - bottom1).abs();
		//	const PxReal eps = 0.001f;
		//	PX_ASSERT(tDif.x < eps && tDif.y < eps && tDif.z < eps);
		//	PX_ASSERT(bDif.x < eps && bDif.y < eps && bDif.z < eps);*/
		//	return Cm::SpatialVectorF(top1, bottom1);
		//}

		//This assume angular is the top vector and linear is the bottom vector
		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF operator *(const Cm::SpatialVectorF& s) const
		{
			//const PxVec3 top = R * s.top;
			//const PxVec3 bottom = T * s.top + R * s.bottom;

			const PxVec3 top1 = q.rotate(s.top);
			const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);

			return Cm::SpatialVectorF(top1, bottom1);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector operator *(const Cm::UnAlignedSpatialVector& s) const
		{
			//const PxVec3 top = R * s.top;
			//const PxVec3 bottom = T * s.top + R * s.bottom;

			const PxVec3 top1 = q.rotate(s.top);
			const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);

			return Cm::UnAlignedSpatialVector(top1, bottom1);
		}

		//transpose is the same as inverse, R(inverse) = R(transpose)
		//|R(t),	0	|
		//|rXR(t),	R(t)|
		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform getTranspose() const
		{
			SpatialTransform ret;
			ret.q = q.getConjugate();
			ret.R = R.getTranspose();
			ret.T = T.getTranspose();
			return ret;
			
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF transposeTransform(const Cm::SpatialVectorF& s) const
		{
			const PxVec3 top1 = q.rotateInv(s.top);
			const PxVec3 bottom1 = T.transformTranspose(s.top) + q.rotateInv(s.bottom);

			return Cm::SpatialVectorF(top1, bottom1);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector transposeTransform(const Cm::UnAlignedSpatialVector& s) const
		{
			const PxVec3 top1 = q.rotateInv(s.top);
			const PxVec3 bottom1 = T.transformTranspose(s.top) + q.rotateInv(s.bottom);

			return Cm::UnAlignedSpatialVector(top1, bottom1);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE void operator =(SpatialTransform& other)
		{
			R = other.R;
			q = other.q;
			T = other.T;
		}

	};

	struct InvStIs
	{
		PxReal invStIs[3][3];
	};

	struct IsInvD
	{
		Cm::SpatialVectorF isInvD[3];
	};

	//this should be 6x6 matrix and initialize to
	//|0,	M|
	//|I,	0|
	//this should be 6x6 matrix but bottomRight is the transpose of topLeft
	//so we can get rid of bottomRight
	struct SpatialMatrix
	{
		PxMat33 topLeft;		// intialize to 0 
		PxMat33 topRight;		// initialize to mass matrix
		PxMat33 bottomLeft;		// initialize to inertia
		PxU32	padding;		//4		112

	public:
		
		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix()
		{
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix(PxZERO r) : topLeft(PxZero), topRight(PxZero),
			bottomLeft(PxZero)
		{
			PX_UNUSED(r);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix(const PxMat33& topLeft_, const PxMat33& topRight_, const PxMat33& bottomLeft_)
		{
			topLeft = topLeft_;
			topRight = topRight_;
			bottomLeft = bottomLeft_;
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 getBottomRight() const
		{
			return topLeft.getTranspose();
		}

		PX_FORCE_INLINE void setZero()
		{
			topLeft = PxMat33(0.f);
			topRight = PxMat33(0.f);
			bottomLeft = PxMat33(0.f);
		}


		//This assume angular is the top vector and linear is the bottom vector
		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVector operator *(const Cm::SpatialVector& s) const
		{
			const PxVec3 angular = topLeft * s.angular + topRight * s.linear;
			const PxVec3 linear = bottomLeft * s.angular + topLeft.getTranspose() * s.linear;
			return Cm::SpatialVector(linear, angular);
		}

		//This assume angular is the top vector and linear is the bottom vector
		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF operator *(const Cm::SpatialVectorF& s) const
		{
			const PxVec3 top = topLeft * s.top + topRight * s.bottom;
			const PxVec3 bottom = bottomLeft * s.top + topLeft.transformTranspose(s.bottom);

			return Cm::SpatialVectorF(top, bottom);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector operator *(const Cm::UnAlignedSpatialVector& s) const
		{
			const PxVec3 top = topLeft * s.top + topRight * s.bottom;
			const PxVec3 bottom = bottomLeft * s.top + topLeft.transformTranspose(s.bottom);

			return Cm::UnAlignedSpatialVector(top, bottom);
		}


		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const PxReal& s) const
		{
			const PxMat33 newTopLeft = topLeft * s;
			const PxMat33 newTopRight = topRight * s;
			const PxMat33 newBottomLeft = bottomLeft * s;

			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator -(const SpatialMatrix& s) const
		{
			PxMat33 newTopLeft = topLeft - s.topLeft;
			PxMat33 newTopRight = topRight - s.topRight;
			PxMat33 newBottomLeft = bottomLeft - s.bottomLeft;
			
			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator +(const SpatialMatrix& s) const
		{
			PxMat33 newTopLeft = topLeft + s.topLeft;
			PxMat33 newTopRight = topRight + s.topRight;
			PxMat33 newBottomLeft = bottomLeft + s.bottomLeft;

			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator-()
		{
			PxMat33 newTopLeft = -topLeft;
			PxMat33 newTopRight = -topRight;
			PxMat33 newBottomLeft = -bottomLeft;
			
			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE void operator +=(const SpatialMatrix& s)
		{
			topLeft += s.topLeft;
			topRight += s.topRight;
			bottomLeft += s.bottomLeft;
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const SpatialMatrix& s)
		{
			PxMat33 sBottomRight = s.topLeft.getTranspose();
			PxMat33 bottomRight = topLeft.getTranspose();

			PxMat33 newTopLeft = topLeft * s.topLeft + topRight * s.bottomLeft;
			PxMat33 newTopRight = topLeft * s.topRight + topRight * sBottomRight;
			PxMat33 newBottomLeft = bottomLeft * s.topLeft + bottomRight * s.bottomLeft;

			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
		}

		static SpatialMatrix constructSpatialMatrix(const Cm::SpatialVector& Is, const Cm::SpatialVector& stI)
		{
			//construct top left
			PxVec3 tLeftC0 = Is.angular * stI.angular.x;
			PxVec3 tLeftC1 = Is.angular * stI.angular.y;
			PxVec3 tLeftC2 = Is.angular * stI.angular.z;

			PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);

			//construct top right
			PxVec3 tRightC0 = Is.angular * stI.linear.x;
			PxVec3 tRightC1 = Is.angular * stI.linear.y;
			PxVec3 tRightC2 = Is.angular * stI.linear.z;
			PxMat33 topRight(tRightC0, tRightC1, tRightC2);

			//construct bottom left
			PxVec3 bLeftC0 = Is.linear * stI.angular.x;
			PxVec3 bLeftC1 = Is.linear * stI.angular.y;
			PxVec3 bLeftC2 = Is.linear * stI.angular.z;
			PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);

			return SpatialMatrix(topLeft, topRight, bottomLeft);
		}

		static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const Cm::SpatialVectorF& Is, const Cm::SpatialVectorF& stI)
		{
			//construct top left
			PxVec3 tLeftC0 = Is.top * stI.top.x;
			PxVec3 tLeftC1 = Is.top * stI.top.y;
			PxVec3 tLeftC2 = Is.top * stI.top.z;

			PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);

			//construct top right
			PxVec3 tRightC0 = Is.top * stI.bottom.x;
			PxVec3 tRightC1 = Is.top * stI.bottom.y;
			PxVec3 tRightC2 = Is.top * stI.bottom.z;
			PxMat33 topRight(tRightC0, tRightC1, tRightC2);

			//construct bottom left
			PxVec3 bLeftC0 = Is.bottom * stI.top.x;
			PxVec3 bLeftC1 = Is.bottom * stI.top.y;
			PxVec3 bLeftC2 = Is.bottom * stI.top.z;
			PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);

			return SpatialMatrix(topLeft, topRight, bottomLeft);
		}

		static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const Cm::SpatialVectorF* columns)
		{
			PxMat33 topLeft(columns[0].top, columns[1].top, columns[2].top);
			PxMat33 bottomLeft(columns[0].bottom, columns[1].bottom, columns[2].bottom);
			PxMat33 topRight(columns[3].top, columns[4].top, columns[5].top);
			
			return SpatialMatrix(topLeft, topRight, bottomLeft);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix getTranspose()
		{
			PxMat33 newTopLeft = topLeft.getTranspose();
			PxMat33 newTopRight = bottomLeft.getTranspose();
			PxMat33 newBottomLeft = topRight.getTranspose();
			//PxMat33 newBottomRight = bottomRight.getTranspose();

			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);// , newBottomRight);
		}

		//static bool isTranspose(const PxMat33& a, const PxMat33& b)
		//{
		//	PxReal eps = 0.01f;
		//	//test bottomRight is the transpose of topLeft
		//	for (PxU32 i = 0; i <3; ++i)
		//	{
		//		for (PxU32 j = 0; j <3; ++j)
		//		{
		//			if (PxAbs(a[i][j] - b[j][i]) > eps)
		//				return false;
		//		}
		//	}

		//	return true;
		//}

		PX_FORCE_INLINE bool isIdentity(const PxMat33& matrix)
		{
			PxReal eps = 0.00001f;
			float x = PxAbs(1.f - matrix.column0.x);
			float y = PxAbs(1.f - matrix.column1.y);
			float z = PxAbs(1.f - matrix.column2.z);
			bool identity = ((x < eps) && PxAbs(matrix.column0.y - 0.f) < eps && PxAbs(matrix.column0.z - 0.f) < eps) &&
				(PxAbs(matrix.column1.x - 0.f) < eps && (y < eps) && PxAbs(matrix.column1.z - 0.f) < eps) &&
				(PxAbs(matrix.column2.x - 0.f) < eps && PxAbs(matrix.column2.y - 0.f) < eps && (z < eps));

			return identity;
		}

		PX_FORCE_INLINE bool isZero(const PxMat33& matrix)
		{
			PxReal eps = 0.0001f;
			for (PxU32 i = 0; i < 3; ++i)
			{
				for (PxU32 j = 0; j < 3; ++j)
				{
					if (PxAbs(matrix[i][j]) > eps)
						return false;
				}
			}

			return true;
		}

		PX_FORCE_INLINE bool isIdentity()
		{
			bool topLeftIsIdentity = isIdentity(topLeft);

			bool topRightIsZero = isZero(topRight);

			bool bottomLeftIsZero = isZero(bottomLeft);

			return topLeftIsIdentity && topRightIsZero && bottomLeftIsZero;
		}

		static bool isEqual(const PxMat33& s0, const PxMat33& s1)
		{
			PxReal eps = 0.00001f;
			for (PxU32 i = 0; i < 3; ++i)
			{
				for (PxU32 j = 0; j < 3; ++j)
				{
					PxReal t = s0[i][j] - s1[i][j];
					if (PxAbs(t) > eps)
						return false;
				}
			}

			return true;
		}

		PX_FORCE_INLINE bool isEqual(const SpatialMatrix& s)
		{
			bool topLeftEqual = isEqual(topLeft, s.topLeft);
			bool topRightEqual = isEqual(topRight, s.topRight);
			bool bottomLeftEqual = isEqual(bottomLeft, s.bottomLeft);

			return topLeftEqual && topRightEqual && bottomLeftEqual;
		}

		static PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 invertSym33(const PxMat33& in)
		{
			PxVec3 v0 = in[1].cross(in[2]),
				v1 = in[2].cross(in[0]),
				v2 = in[0].cross(in[1]);

			PxReal det = v0.dot(in[0]);

			if (det != 0)
			{
				PxReal recipDet = 1.0f / det;

				return PxMat33(v0 * recipDet,
					PxVec3(v0.y, v1.y, v1.z) * recipDet,
					PxVec3(v0.z, v1.z, v2.z) * recipDet);
			}
			else
			{
				return PxMat33(PxIdentity);
			}
		}

		static PX_FORCE_INLINE Ps::aos::Mat33V invertSym33(const Ps::aos::Mat33V& in)
		{
			using namespace Ps::aos;
			const Vec3V v0 = V3Cross(in.col1, in.col2);
			const Vec3V v1 = V3Cross(in.col2, in.col0);
			const Vec3V v2 = V3Cross(in.col0, in.col1);

			FloatV det = V3Dot(v0, in.col0);

			const FloatV recipDet = FRecip(det);

			if (!FAllEq(det, FZero()))
			{
				return Mat33V(V3Scale(v0, recipDet),
					V3Scale(V3Merge(V3GetY(v0), V3GetY(v1), V3GetZ(v1)), recipDet),
					V3Scale(V3Merge(V3GetZ(v0), V3GetZ(v1), V3GetZ(v2)), recipDet));
			}
			else
			{
				return Mat33V(V3UnitX(), V3UnitY(), V3UnitZ());
			}

			//return M33Inverse(in);
		}

		PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix invertInertia()
		{
			PxMat33 aa = bottomLeft, ll = topRight, la = topLeft;

			aa = (aa + aa.getTranspose())*0.5f;
			ll = (ll + ll.getTranspose())*0.5f;

			PxMat33 AAInv = invertSym33(aa);

			PxMat33 z = -la * AAInv;
			PxMat33 S = ll + z * la.getTranspose();	// Schur complement of mAA

			PxMat33 LL = invertSym33(S);

			PxMat33 LA = LL * z;
			PxMat33 AA = AAInv + z.getTranspose() * LA;

			SpatialMatrix result(LA.getTranspose(), AA, LL);// , LA);

			return result;
		}

		PX_FORCE_INLINE void M33Store(const Ps::aos::Mat33V& src, PxMat33& dest)
		{
			Ps::aos::V3StoreU(src.col0, dest.column0);
			Ps::aos::V3StoreU(src.col1, dest.column1);
			Ps::aos::V3StoreU(src.col2, dest.column2);
		}

		PX_FORCE_INLINE void invertInertiaV(SpatialMatrix& result)
		{
			using namespace Ps::aos;
			Mat33V aa = M33Load(bottomLeft), ll = M33Load(topRight), la = M33Load(topLeft);

			aa = M33Scale(M33Add(aa, M33Trnsps(aa)), FHalf());
			ll = M33Scale(M33Add(ll, M33Trnsps(ll)), FHalf());

			Mat33V AAInv = invertSym33(aa);

			Mat33V z = M33MulM33(M33Neg(la), AAInv);
			Mat33V S = M33Add(ll, M33MulM33(z, M33Trnsps(la)));	// Schur complement of mAA

			Mat33V LL = invertSym33(S);

			Mat33V LA = M33MulM33(LL, z);
			Mat33V AA = M33Add(AAInv, M33MulM33(M33Trnsps(z), LA));

			M33Store(M33Trnsps(LA), result.topLeft);
			M33Store(AA, result.topRight);
			M33Store(LL, result.bottomLeft);
		}

		SpatialMatrix getInverse()
		{
			PxMat33 bottomRight = topLeft.getTranspose();

			PxMat33 blInverse = bottomLeft.getInverse();
			PxMat33 lComp0 = blInverse * (-bottomRight);
			PxMat33 lComp1 = topLeft * lComp0 + topRight;

			//This can be simplified
			PxMat33 newBottomLeft = lComp1.getInverse();
			PxMat33 newTopLeft = lComp0 * newBottomLeft;

			PxMat33 trInverse = topRight.getInverse();
			PxMat33 rComp0 = trInverse * (-topLeft);
			PxMat33 rComp1 = bottomLeft + bottomRight * rComp0;

			PxMat33 newTopRight = rComp1.getInverse();
			
			return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
		}

		void zero()
		{
			topLeft = PxMat33(PxZero);
			topRight = PxMat33(PxZero);
			bottomLeft = PxMat33(PxZero);
		}

	};

	struct SpatialImpulseResponseMatrix
	{
		Cm::SpatialVectorF rows[6];

		Cm::SpatialVectorF getResponse(const Cm::SpatialVectorF& impulse) const
		{
			/*return rows[0] * impulse.top.x + rows[1] * impulse.top.y + rows[2] * impulse.top.z
			+ rows[3] * impulse.bottom.x + rows[4] * impulse.bottom.y + rows[5] * impulse.bottom.z;*/

			using namespace Ps::aos;
			Cm::SpatialVectorV row0(V3LoadA(&rows[0].top.x), V3LoadA(&rows[0].bottom.x));
			Cm::SpatialVectorV row1(V3LoadA(&rows[1].top.x), V3LoadA(&rows[1].bottom.x));
			Cm::SpatialVectorV row2(V3LoadA(&rows[2].top.x), V3LoadA(&rows[2].bottom.x));
			Cm::SpatialVectorV row3(V3LoadA(&rows[3].top.x), V3LoadA(&rows[3].bottom.x));
			Cm::SpatialVectorV row4(V3LoadA(&rows[4].top.x), V3LoadA(&rows[4].bottom.x));
			Cm::SpatialVectorV row5(V3LoadA(&rows[5].top.x), V3LoadA(&rows[5].bottom.x));

			Vec4V top = V4LoadA(&impulse.top.x);
			Vec4V bottom = V4LoadA(&impulse.bottom.x);

			const FloatV ix = V4GetX(top);
			const FloatV iy = V4GetY(top);
			const FloatV iz = V4GetZ(top);
			const FloatV ia = V4GetX(bottom);
			const FloatV ib = V4GetY(bottom);
			const FloatV ic = V4GetZ(bottom);

			Cm::SpatialVectorV res =  row0 * ix + row1 * iy + row2 * iz + row3 * ia + row4 * ib + row5 * ic;

			Cm::SpatialVectorF returnVal;
			V4StoreA(Vec4V_From_Vec3V(res.linear), &returnVal.top.x);
			V4StoreA(Vec4V_From_Vec3V(res.angular), &returnVal.bottom.x);

			return returnVal;

		}

		Cm::SpatialVectorV getResponse(const Cm::SpatialVectorV& impulse) const
		{
			using namespace Ps::aos;
			Cm::SpatialVectorV row0(V3LoadA(&rows[0].top.x), V3LoadA(&rows[0].bottom.x));
			Cm::SpatialVectorV row1(V3LoadA(&rows[1].top.x), V3LoadA(&rows[1].bottom.x));
			Cm::SpatialVectorV row2(V3LoadA(&rows[2].top.x), V3LoadA(&rows[2].bottom.x));
			Cm::SpatialVectorV row3(V3LoadA(&rows[3].top.x), V3LoadA(&rows[3].bottom.x));
			Cm::SpatialVectorV row4(V3LoadA(&rows[4].top.x), V3LoadA(&rows[4].bottom.x));
			Cm::SpatialVectorV row5(V3LoadA(&rows[5].top.x), V3LoadA(&rows[5].bottom.x));

			const Vec3V top = impulse.linear;
			const Vec3V bottom = impulse.angular;

			const FloatV ix = V3GetX(top);
			const FloatV iy = V3GetY(top);
			const FloatV iz = V3GetZ(top);
			const FloatV ia = V3GetX(bottom);
			const FloatV ib = V3GetY(bottom);
			const FloatV ic = V3GetZ(bottom);

			Cm::SpatialVectorV res = row0 * ix + row1 * iy + row2 * iz + row3 * ia + row4 * ib + row5 * ic;
			return res;
		}
	};

	struct Temp6x6Matrix;

	struct Temp6x3Matrix
	{
		PxReal column[3][6];
	public:

		Temp6x3Matrix()
		{

		}

		Temp6x3Matrix(const Cm::SpatialVectorF* spatialAxis)
		{
			constructColumn(column[0], spatialAxis[0]);
			constructColumn(column[1], spatialAxis[1]);
			constructColumn(column[2], spatialAxis[2]);
		}

		void constructColumn(PxReal* dest, const Cm::SpatialVectorF& v)
		{
			dest[0] = v.top.x;
			dest[1] = v.top.y;
			dest[2] = v.top.z;

			dest[3] = v.bottom.x;
			dest[4] = v.bottom.y;
			dest[5] = v.bottom.z;
		}

		Temp6x6Matrix operator * (PxReal s[6][3]);

		////s is 3x6 matrix
		//PX_FORCE_INLINE Temp6x6Matrix operator * (PxReal s[6][3])
		//{
		//	Temp6x6Matrix temp;

		//	for (PxU32 i = 0; i < 6; ++i)
		//	{
		//		PxReal* tc = temp.column[i];

		//		for (PxU32 j = 0; j < 6; ++j)
		//		{
		//			tc[j] = 0.f;
		//			for (PxU32 k = 0; k < 3; ++k)
		//			{
		//				tc[j] += column[k][j] * s[i][k];
		//			}
		//		}
		//	}

		//	return temp;
		//}

		PX_FORCE_INLINE Temp6x3Matrix operator * (const PxMat33& s)
		{
			Temp6x3Matrix temp;

			for (PxU32 i = 0; i < 3; ++i)
			{
				PxReal* tc = temp.column[i];
				PxVec3 sc = s[i];

				for (PxU32 j = 0; j < 6; ++j)
				{
					tc[j] = 0.f;
					for (PxU32 k = 0; k < 3; ++k)
					{
						tc[j] += column[k][j] * sc[k];
					}
				}
			}

			return temp;
		}

		PX_FORCE_INLINE bool isColumnEqual(const PxU32 ind, const Cm::SpatialVectorF& col)
		{
			PxReal temp[6];
			constructColumn(temp, col);
			const PxReal eps = 0.00001f;
			for (PxU32 i = 0; i < 6; ++i)
			{
				const PxReal dif = column[ind][i] - temp[i];
				if (PxAbs(dif) > eps)
					return false;
			}
			return true;
		}

	};


	struct Temp6x6Matrix
	{
		PxReal column[6][6];
	public:
		Temp6x6Matrix()
		{

		}

		Temp6x6Matrix(const SpatialMatrix& spatialMatrix)
		{
			constructColumn(column[0], spatialMatrix.topLeft.column0, spatialMatrix.bottomLeft.column0);
			constructColumn(column[1], spatialMatrix.topLeft.column1, spatialMatrix.bottomLeft.column1);
			constructColumn(column[2], spatialMatrix.topLeft.column2, spatialMatrix.bottomLeft.column2);

			const PxMat33 bottomRight = spatialMatrix.getBottomRight();
			constructColumn(column[3], spatialMatrix.topRight.column0, bottomRight.column0);
			constructColumn(column[4], spatialMatrix.topRight.column1, bottomRight.column1);
			constructColumn(column[5], spatialMatrix.topRight.column2, bottomRight.column2);
		}

		void constructColumn(const PxU32 ind, const PxReal* const values)
		{
			for (PxU32 i = 0; i < 6; ++i)
			{
				column[ind][i] = values[i];
			}
		}

		void constructColumn(PxReal* dest, const PxVec3& top, const PxVec3& bottom)
		{
			dest[0] = top.x;
			dest[1] = top.y;
			dest[2] = top.z;

			dest[3] = bottom.x;
			dest[4] = bottom.y;
			dest[5] = bottom.z;
		}

		Temp6x6Matrix getTranspose() const
		{
			Temp6x6Matrix temp;
			for (PxU32 i = 0; i < 6; ++i)
			{
				for (PxU32 j = 0; j < 6; ++j)
				{
					temp.column[i][j] = column[j][i];
				}
			}
			return temp;
		}

		PX_FORCE_INLINE Cm::SpatialVector operator * (const Cm::SpatialVector& s) const
		{
			Temp6x6Matrix tempMatrix = getTranspose();
			PxReal st[6];
			st[0] = s.angular.x; st[1] = s.angular.y; st[2] = s.angular.z;
			st[3] = s.linear.x; st[4] = s.linear.y; st[5] = s.linear.z;

			PxReal result[6];
			for (PxU32 i = 0; i < 6; i++)
			{
				result[i] = 0;
				for (PxU32 j = 0; j < 6; ++j)
				{
					result[i] += tempMatrix.column[i][j] * st[j];
				}
			}


			Cm::SpatialVector temp;
			temp.angular.x = result[0]; temp.angular.y = result[1]; temp.angular.z = result[2];
			temp.linear.x = result[3]; temp.linear.y = result[4]; temp.linear.z = result[5];
			return temp;
		}


		PX_FORCE_INLINE Cm::SpatialVectorF operator * (const Cm::SpatialVectorF& s) const
		{
			PxReal st[6];
			st[0] = s.top.x; st[1] = s.top.y; st[2] = s.top.z;
			st[3] = s.bottom.x; st[4] = s.bottom.y; st[5] = s.bottom.z;

			PxReal result[6];
			for (PxU32 i = 0; i < 6; ++i)
			{
				result[i] = 0.f;
				for (PxU32 j = 0; j < 6; ++j)
				{
					result[i] += column[j][i] * st[j];
				}
			}

			Cm::SpatialVectorF temp;
			temp.top.x = result[0]; temp.top.y = result[1]; temp.top.z = result[2];
			temp.bottom.x = result[3]; temp.bottom.y = result[4]; temp.bottom.z = result[5];
			return temp;
		}

		PX_FORCE_INLINE Temp6x3Matrix operator * (const Temp6x3Matrix& s) const
		{
			Temp6x3Matrix temp;
			for (PxU32 i = 0; i < 3; ++i)
			{
				PxReal* result = temp.column[i];

				const PxReal* input = s.column[i];

				for (PxU32 j = 0; j < 6; ++j)
				{
					result[j] = 0.f;
					for (PxU32 k = 0; k < 6; ++k)
					{
						result[j] += column[k][j] * input[k];
					}
				}
			}

			return temp;

		}

		PX_FORCE_INLINE Cm::SpatialVector spatialVectorMul(const Cm::SpatialVector& s)
		{
			PxReal st[6];
			st[0] = s.angular.x; st[1] = s.angular.y; st[2] = s.angular.z;
			st[3] = s.linear.x; st[4] = s.linear.y; st[5] = s.linear.z;

			PxReal result[6];
			for (PxU32 i = 0; i < 6; ++i)
			{
				result[i] = 0.f;
				for (PxU32 j = 0; j < 6; j++)
				{
					result[i] += column[i][j] * st[j];
				}
			}

			Cm::SpatialVector temp;
			temp.angular.x = result[0]; temp.angular.y = result[1]; temp.angular.z = result[2];
			temp.linear.x = result[3]; temp.linear.y = result[4]; temp.linear.z = result[5];
			return temp;
		}

		PX_FORCE_INLINE bool isEqual(const Cm::SpatialVectorF* m)
		{
			PxReal temp[6];
			PxReal eps = 0.00001f;
			for (PxU32 i = 0; i < 6; ++i)
			{
				temp[0] = m[i].top.x; temp[1] = m[i].top.y; temp[2] = m[i].top.z;
				temp[3] = m[i].bottom.x; temp[4] = m[i].bottom.y; temp[5] = m[i].bottom.z;

				for (PxU32 j = 0; j < 6; ++j)
				{
					PxReal dif = column[i][j] - temp[j];
					if (PxAbs(dif) > eps)
						return false;
				}
			}

			return true;
		}
	};

	//s is 3x6 matrix
	PX_FORCE_INLINE Temp6x6Matrix Temp6x3Matrix::operator * (PxReal s[6][3])
	{
		Temp6x6Matrix temp;

		for (PxU32 i = 0; i < 6; ++i)
		{
			PxReal* tc = temp.column[i];

			for (PxU32 j = 0; j < 6; ++j)
			{
				tc[j] = 0.f;
				for (PxU32 k = 0; k < 3; ++k)
				{
					tc[j] += column[k][j] * s[i][k];
				}
			}
		}

		return temp;
	}

	PX_FORCE_INLINE void calculateNewVelocity(const PxTransform& newTransform, const PxTransform& oldTransform,
		const PxReal dt, PxVec3& linear, PxVec3& angular)
	{
		//calculate the new velocity
		linear = (newTransform.p - oldTransform.p) / dt;
		PxQuat quat = newTransform.q * oldTransform.q.getConjugate();

		if (quat.w < 0)	//shortest angle.
			quat = -quat;

		PxReal angle;
		PxVec3 axis;
		quat.toRadiansAndUnitAxis(angle, axis);
		angular = (axis * angle) / dt;
	}


	// generates a pair of quaternions (swing, twist) such that in = swing * twist, with
	// swing.x = 0
	// twist.y = twist.z = 0, and twist is a unit quat
	PX_CUDA_CALLABLE PX_FORCE_INLINE void separateSwingTwist(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
	{
		twist = q.x != 0.0f ? PxQuat(q.x, 0, 0, q.w).getNormalized() : PxQuat(PxIdentity);
		PxQuat swing = q * twist.getConjugate();
		swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
		swing = swing * swing1.getConjugate();
		swing2 = swing.z != 0.f ? PxQuat(0.f, 0.f, swing.z, swing.w).getNormalized() : PxQuat(PxIdentity);
	}

	PX_CUDA_CALLABLE PX_FORCE_INLINE void separateSwingTwist2(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
	{
		swing2 = q.z != 0.0f ? PxQuat(0.f, 0.f, q.z, q.w).getNormalized() : PxQuat(PxIdentity);
		PxQuat swing = q * swing2.getConjugate();
		swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
		swing = swing * swing1.getConjugate();
		twist = swing.x != 0.f ? PxQuat(swing.x, 0.f, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
	}


} //namespace Dy

}

#endif