PageRenderTime 22ms CodeModel.GetById 2ms app.highlight 16ms RepoModel.GetById 2ms app.codeStats 0ms

/indra/llmath/llv4matrix4.h

https://bitbucket.org/lindenlab/viewer-beta/
C Header | 249 lines | 161 code | 43 blank | 45 comment | 1 complexity | 5af2d25193d3c0f74f5cfac279f80f2b MD5 | raw file
  1/** 
  2 * @file llviewerjointmesh.cpp
  3 * @brief LLV4* class header file - vector processor enabled math
  4 *
  5 * $LicenseInfo:firstyear=2007&license=viewerlgpl$
  6 * Second Life Viewer Source Code
  7 * Copyright (C) 2010, Linden Research, Inc.
  8 * 
  9 * This library is free software; you can redistribute it and/or
 10 * modify it under the terms of the GNU Lesser General Public
 11 * License as published by the Free Software Foundation;
 12 * version 2.1 of the License only.
 13 * 
 14 * This library is distributed in the hope that it will be useful,
 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 17 * Lesser General Public License for more details.
 18 * 
 19 * You should have received a copy of the GNU Lesser General Public
 20 * License along with this library; if not, write to the Free Software
 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 22 * 
 23 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 24 * $/LicenseInfo$
 25 */
 26
 27#ifndef LL_LLV4MATRIX4_H
 28#define LL_LLV4MATRIX4_H
 29
 30#include "llv4math.h"
 31#include "llv4matrix3.h"	// just for operator LLV4Matrix3()
 32#include "llv4vector3.h"
 33
 34//-----------------------------------------------------------------------------
 35//-----------------------------------------------------------------------------
 36// LLV4Matrix4
 37//-----------------------------------------------------------------------------
 38//-----------------------------------------------------------------------------
 39
 40LL_LLV4MATH_ALIGN_PREFIX
 41
 42class LLV4Matrix4
 43{
 44public:
 45	union {
 46		F32		mMatrix[LLV4_NUM_AXIS][LLV4_NUM_AXIS];
 47		V4F32	mV[LLV4_NUM_AXIS];
 48	};
 49
 50	void				lerp(const LLV4Matrix4 &a, const LLV4Matrix4 &b, const F32 &w);
 51	void				multiply(const LLVector3 &a, LLVector3& o) const;
 52	void				multiply(const LLVector3 &a, LLV4Vector3& o) const;
 53
 54	const LLV4Matrix4&	transpose();
 55	const LLV4Matrix4&  translate(const LLVector3 &vec);
 56	const LLV4Matrix4&  translate(const LLV4Vector3 &vec);
 57	const LLV4Matrix4&	operator=(const LLMatrix4& a);
 58
 59	operator			LLMatrix4()	const { return *(reinterpret_cast<const LLMatrix4*>(const_cast<const F32*>(&mMatrix[0][0]))); }
 60	operator			LLV4Matrix3()	const { return *(reinterpret_cast<const LLV4Matrix3*>(const_cast<const F32*>(&mMatrix[0][0]))); }
 61	
 62	friend LLVector3	operator*(const LLVector3 &a, const LLV4Matrix4 &b);
 63}
 64
 65LL_LLV4MATH_ALIGN_POSTFIX;
 66
 67//-----------------------------------------------------------------------------
 68//-----------------------------------------------------------------------------
 69// LLV4Matrix4 - SSE
 70//-----------------------------------------------------------------------------
 71//-----------------------------------------------------------------------------
 72
 73#if LL_VECTORIZE
 74
 75inline void LLV4Matrix4::lerp(const LLV4Matrix4 &a, const LLV4Matrix4 &b, const F32 &w)
 76{
 77	__m128 vw = _mm_set1_ps(w);
 78	mV[VX] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VX], a.mV[VX]), vw), a.mV[VX]); // ( b - a ) * w + a
 79	mV[VY] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VY], a.mV[VY]), vw), a.mV[VY]);
 80	mV[VZ] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VZ], a.mV[VZ]), vw), a.mV[VZ]);
 81	mV[VW] = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(b.mV[VW], a.mV[VW]), vw), a.mV[VW]);
 82}
 83
 84inline void LLV4Matrix4::multiply(const LLVector3 &a, LLVector3& o) const
 85{
 86	LLV4Vector3 j;
 87	j.v = _mm_add_ps(mV[VW], _mm_mul_ps(_mm_set1_ps(a.mV[VX]), mV[VX])); // ( ax * vx ) + vw
 88	j.v = _mm_add_ps(j.v   , _mm_mul_ps(_mm_set1_ps(a.mV[VY]), mV[VY]));
 89	j.v = _mm_add_ps(j.v   , _mm_mul_ps(_mm_set1_ps(a.mV[VZ]), mV[VZ]));
 90	o.setVec(j.mV);
 91}
 92
 93inline void LLV4Matrix4::multiply(const LLVector3 &a, LLV4Vector3& o) const
 94{
 95	o.v = _mm_add_ps(mV[VW], _mm_mul_ps(_mm_set1_ps(a.mV[VX]), mV[VX])); // ( ax * vx ) + vw
 96	o.v = _mm_add_ps(o.v   , _mm_mul_ps(_mm_set1_ps(a.mV[VY]), mV[VY]));
 97	o.v = _mm_add_ps(o.v   , _mm_mul_ps(_mm_set1_ps(a.mV[VZ]), mV[VZ]));
 98}
 99
100inline const LLV4Matrix4& LLV4Matrix4::translate(const LLV4Vector3 &vec)
101{
102	mV[VW] = _mm_add_ps(mV[VW], vec.v);
103	return (*this);
104}
105
106//-----------------------------------------------------------------------------
107//-----------------------------------------------------------------------------
108// LLV4Matrix4
109//-----------------------------------------------------------------------------
110//-----------------------------------------------------------------------------
111
112#else
113
114inline void LLV4Matrix4::lerp(const LLV4Matrix4 &a, const LLV4Matrix4 &b, const F32 &w)
115{
116	mMatrix[VX][VX] = llv4lerp(a.mMatrix[VX][VX], b.mMatrix[VX][VX], w);
117	mMatrix[VX][VY] = llv4lerp(a.mMatrix[VX][VY], b.mMatrix[VX][VY], w);
118	mMatrix[VX][VZ] = llv4lerp(a.mMatrix[VX][VZ], b.mMatrix[VX][VZ], w);
119
120	mMatrix[VY][VX] = llv4lerp(a.mMatrix[VY][VX], b.mMatrix[VY][VX], w);
121	mMatrix[VY][VY] = llv4lerp(a.mMatrix[VY][VY], b.mMatrix[VY][VY], w);
122	mMatrix[VY][VZ] = llv4lerp(a.mMatrix[VY][VZ], b.mMatrix[VY][VZ], w);
123
124	mMatrix[VZ][VX] = llv4lerp(a.mMatrix[VZ][VX], b.mMatrix[VZ][VX], w);
125	mMatrix[VZ][VY] = llv4lerp(a.mMatrix[VZ][VY], b.mMatrix[VZ][VY], w);
126	mMatrix[VZ][VZ] = llv4lerp(a.mMatrix[VZ][VZ], b.mMatrix[VZ][VZ], w);
127
128	mMatrix[VW][VX] = llv4lerp(a.mMatrix[VW][VX], b.mMatrix[VW][VX], w);
129	mMatrix[VW][VY] = llv4lerp(a.mMatrix[VW][VY], b.mMatrix[VW][VY], w);
130	mMatrix[VW][VZ] = llv4lerp(a.mMatrix[VW][VZ], b.mMatrix[VW][VZ], w);
131}
132
133inline void LLV4Matrix4::multiply(const LLVector3 &a, LLVector3& o) const
134{
135	o.setVec(		a.mV[VX] * mMatrix[VX][VX] + 
136					a.mV[VY] * mMatrix[VY][VX] + 
137					a.mV[VZ] * mMatrix[VZ][VX] +
138					mMatrix[VW][VX],
139					 
140					a.mV[VX] * mMatrix[VX][VY] + 
141					a.mV[VY] * mMatrix[VY][VY] + 
142					a.mV[VZ] * mMatrix[VZ][VY] +
143					mMatrix[VW][VY],
144					 
145					a.mV[VX] * mMatrix[VX][VZ] + 
146					a.mV[VY] * mMatrix[VY][VZ] + 
147					a.mV[VZ] * mMatrix[VZ][VZ] +
148					mMatrix[VW][VZ]);
149}
150
151inline void LLV4Matrix4::multiply(const LLVector3 &a, LLV4Vector3& o) const
152{
153	o.setVec(		a.mV[VX] * mMatrix[VX][VX] + 
154					a.mV[VY] * mMatrix[VY][VX] + 
155					a.mV[VZ] * mMatrix[VZ][VX] +
156					mMatrix[VW][VX],
157					 
158					a.mV[VX] * mMatrix[VX][VY] + 
159					a.mV[VY] * mMatrix[VY][VY] + 
160					a.mV[VZ] * mMatrix[VZ][VY] +
161					mMatrix[VW][VY],
162					 
163					a.mV[VX] * mMatrix[VX][VZ] + 
164					a.mV[VY] * mMatrix[VY][VZ] + 
165					a.mV[VZ] * mMatrix[VZ][VZ] +
166					mMatrix[VW][VZ]);
167}
168
169inline const LLV4Matrix4& LLV4Matrix4::translate(const LLV4Vector3 &vec)
170{
171	mMatrix[3][0] += vec.mV[0];
172	mMatrix[3][1] += vec.mV[1];
173	mMatrix[3][2] += vec.mV[2];
174	return (*this);
175}
176
177//-----------------------------------------------------------------------------
178//-----------------------------------------------------------------------------
179// LLV4Matrix4
180//-----------------------------------------------------------------------------
181//-----------------------------------------------------------------------------
182
183#endif
184
185inline const LLV4Matrix4& LLV4Matrix4::operator=(const LLMatrix4& a)
186{
187	memcpy(mMatrix, a.mMatrix, sizeof(F32) * 16 );
188	return *this;
189}
190
191inline const LLV4Matrix4& LLV4Matrix4::transpose()
192{
193#if LL_VECTORIZE && defined(_MM_TRANSPOSE4_PS)
194	_MM_TRANSPOSE4_PS(mV[VX], mV[VY], mV[VZ], mV[VW]);
195#else
196	LLV4Matrix4 mat;
197	mat.mMatrix[0][0] = mMatrix[0][0];
198	mat.mMatrix[1][0] = mMatrix[0][1];
199	mat.mMatrix[2][0] = mMatrix[0][2];
200	mat.mMatrix[3][0] = mMatrix[0][3];
201
202	mat.mMatrix[0][1] = mMatrix[1][0];
203	mat.mMatrix[1][1] = mMatrix[1][1];
204	mat.mMatrix[2][1] = mMatrix[1][2];
205	mat.mMatrix[3][1] = mMatrix[1][3];
206
207	mat.mMatrix[0][2] = mMatrix[2][0];
208	mat.mMatrix[1][2] = mMatrix[2][1];
209	mat.mMatrix[2][2] = mMatrix[2][2];
210	mat.mMatrix[3][2] = mMatrix[2][3];
211
212	mat.mMatrix[0][3] = mMatrix[3][0];
213	mat.mMatrix[1][3] = mMatrix[3][1];
214	mat.mMatrix[2][3] = mMatrix[3][2];
215	mat.mMatrix[3][3] = mMatrix[3][3];
216
217	*this = mat;
218#endif
219	return *this;
220}
221
222inline const LLV4Matrix4& LLV4Matrix4::translate(const LLVector3 &vec)
223{
224	mMatrix[3][0] += vec.mV[0];
225	mMatrix[3][1] += vec.mV[1];
226	mMatrix[3][2] += vec.mV[2];
227	return (*this);
228}
229
230inline LLVector3 operator*(const LLVector3 &a, const LLV4Matrix4 &b)
231{
232	return LLVector3(a.mV[VX] * b.mMatrix[VX][VX] + 
233					 a.mV[VY] * b.mMatrix[VY][VX] + 
234					 a.mV[VZ] * b.mMatrix[VZ][VX] +
235					 b.mMatrix[VW][VX],
236					 
237					 a.mV[VX] * b.mMatrix[VX][VY] + 
238					 a.mV[VY] * b.mMatrix[VY][VY] + 
239					 a.mV[VZ] * b.mMatrix[VZ][VY] +
240					 b.mMatrix[VW][VY],
241					 
242					 a.mV[VX] * b.mMatrix[VX][VZ] + 
243					 a.mV[VY] * b.mMatrix[VY][VZ] + 
244					 a.mV[VZ] * b.mMatrix[VZ][VZ] +
245					 b.mMatrix[VW][VZ]);
246}
247
248
249#endif