PageRenderTime 101ms CodeModel.GetById 2ms app.highlight 92ms RepoModel.GetById 1ms app.codeStats 0ms

/indra/llmath/m4math.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 882 lines | 658 code | 156 blank | 68 comment | 20 complexity | 994ba95d813443312b3d463f0e6be527 MD5 | raw file
  1/** 
  2 * @file m4math.cpp
  3 * @brief LLMatrix4 class implementation.
  4 *
  5 * $LicenseInfo:firstyear=2000&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#include "linden_common.h"
 28
 29//#include "vmath.h"
 30#include "v3math.h"
 31#include "v4math.h"
 32#include "m4math.h"
 33#include "m3math.h"
 34#include "llquaternion.h"
 35
 36
 37
 38
 39// LLMatrix4
 40
 41// Constructors
 42
 43
 44LLMatrix4::LLMatrix4(const F32 *mat)
 45{
 46	mMatrix[0][0] = mat[0];
 47	mMatrix[0][1] = mat[1];
 48	mMatrix[0][2] = mat[2];
 49	mMatrix[0][3] = mat[3];
 50
 51	mMatrix[1][0] = mat[4];
 52	mMatrix[1][1] = mat[5];
 53	mMatrix[1][2] = mat[6];
 54	mMatrix[1][3] = mat[7];
 55
 56	mMatrix[2][0] = mat[8];
 57	mMatrix[2][1] = mat[9];
 58	mMatrix[2][2] = mat[10];
 59	mMatrix[2][3] = mat[11];
 60
 61	mMatrix[3][0] = mat[12];
 62	mMatrix[3][1] = mat[13];
 63	mMatrix[3][2] = mat[14];
 64	mMatrix[3][3] = mat[15];
 65}
 66
 67LLMatrix4::LLMatrix4(const LLMatrix3 &mat, const LLVector4 &vec)
 68{
 69	mMatrix[0][0] = mat.mMatrix[0][0];
 70	mMatrix[0][1] = mat.mMatrix[0][1];
 71	mMatrix[0][2] = mat.mMatrix[0][2];
 72	mMatrix[0][3] = 0.f;
 73
 74	mMatrix[1][0] = mat.mMatrix[1][0];
 75	mMatrix[1][1] = mat.mMatrix[1][1];
 76	mMatrix[1][2] = mat.mMatrix[1][2];
 77	mMatrix[1][3] = 0.f;
 78
 79	mMatrix[2][0] = mat.mMatrix[2][0];
 80	mMatrix[2][1] = mat.mMatrix[2][1];
 81	mMatrix[2][2] = mat.mMatrix[2][2];
 82	mMatrix[2][3] = 0.f;
 83
 84	mMatrix[3][0] = vec.mV[0];
 85	mMatrix[3][1] = vec.mV[1];
 86	mMatrix[3][2] = vec.mV[2];
 87	mMatrix[3][3] = 1.f;
 88}
 89
 90LLMatrix4::LLMatrix4(const LLMatrix3 &mat)
 91{
 92	mMatrix[0][0] = mat.mMatrix[0][0];
 93	mMatrix[0][1] = mat.mMatrix[0][1];
 94	mMatrix[0][2] = mat.mMatrix[0][2];
 95	mMatrix[0][3] = 0.f;
 96
 97	mMatrix[1][0] = mat.mMatrix[1][0];
 98	mMatrix[1][1] = mat.mMatrix[1][1];
 99	mMatrix[1][2] = mat.mMatrix[1][2];
100	mMatrix[1][3] = 0.f;
101
102	mMatrix[2][0] = mat.mMatrix[2][0];
103	mMatrix[2][1] = mat.mMatrix[2][1];
104	mMatrix[2][2] = mat.mMatrix[2][2];
105	mMatrix[2][3] = 0.f;
106
107	mMatrix[3][0] = 0.f;
108	mMatrix[3][1] = 0.f;
109	mMatrix[3][2] = 0.f;
110	mMatrix[3][3] = 1.f;
111}
112
113LLMatrix4::LLMatrix4(const LLQuaternion &q)
114{
115	*this = initRotation(q);
116}
117
118LLMatrix4::LLMatrix4(const LLQuaternion &q, const LLVector4 &pos)
119{
120	*this = initRotTrans(q, pos);
121}
122
123LLMatrix4::LLMatrix4(const F32 angle, const LLVector4 &vec, const LLVector4 &pos)
124{
125	initRotTrans(LLQuaternion(angle, vec), pos);
126}
127
128LLMatrix4::LLMatrix4(const F32 angle, const LLVector4 &vec)
129{
130	initRotation(LLQuaternion(angle, vec));
131
132	mMatrix[3][0] = 0.f;
133	mMatrix[3][1] = 0.f;
134	mMatrix[3][2] = 0.f;
135	mMatrix[3][3] = 1.f;
136}
137
138LLMatrix4::LLMatrix4(const F32 roll, const F32 pitch, const F32 yaw, const LLVector4 &pos)
139{
140	LLMatrix3	mat(roll, pitch, yaw);
141	initRotTrans(LLQuaternion(mat), pos);
142}
143
144LLMatrix4::LLMatrix4(const F32 roll, const F32 pitch, const F32 yaw)
145{
146	LLMatrix3	mat(roll, pitch, yaw);
147	initRotation(LLQuaternion(mat));
148
149	mMatrix[3][0] = 0.f;
150	mMatrix[3][1] = 0.f;
151	mMatrix[3][2] = 0.f;
152	mMatrix[3][3] = 1.f;
153}
154
155LLMatrix4::~LLMatrix4(void)
156{
157}
158
159// Clear and Assignment Functions
160
161const LLMatrix4& LLMatrix4::setZero()
162{
163	mMatrix[0][0] = 0.f;
164	mMatrix[0][1] = 0.f;
165	mMatrix[0][2] = 0.f;
166	mMatrix[0][3] = 0.f;
167
168	mMatrix[1][0] = 0.f;
169	mMatrix[1][1] = 0.f;
170	mMatrix[1][2] = 0.f;
171	mMatrix[1][3] = 0.f;
172
173	mMatrix[2][0] = 0.f;
174	mMatrix[2][1] = 0.f;
175	mMatrix[2][2] = 0.f;
176	mMatrix[2][3] = 0.f;
177
178	mMatrix[3][0] = 0.f;
179	mMatrix[3][1] = 0.f;
180	mMatrix[3][2] = 0.f;
181	mMatrix[3][3] = 0.f;
182	return *this;
183}
184
185
186// various useful mMatrix functions
187
188const LLMatrix4&	LLMatrix4::transpose()
189{
190	LLMatrix4 mat;
191	mat.mMatrix[0][0] = mMatrix[0][0];
192	mat.mMatrix[1][0] = mMatrix[0][1];
193	mat.mMatrix[2][0] = mMatrix[0][2];
194	mat.mMatrix[3][0] = mMatrix[0][3];
195
196	mat.mMatrix[0][1] = mMatrix[1][0];
197	mat.mMatrix[1][1] = mMatrix[1][1];
198	mat.mMatrix[2][1] = mMatrix[1][2];
199	mat.mMatrix[3][1] = mMatrix[1][3];
200
201	mat.mMatrix[0][2] = mMatrix[2][0];
202	mat.mMatrix[1][2] = mMatrix[2][1];
203	mat.mMatrix[2][2] = mMatrix[2][2];
204	mat.mMatrix[3][2] = mMatrix[2][3];
205
206	mat.mMatrix[0][3] = mMatrix[3][0];
207	mat.mMatrix[1][3] = mMatrix[3][1];
208	mat.mMatrix[2][3] = mMatrix[3][2];
209	mat.mMatrix[3][3] = mMatrix[3][3];
210
211	*this = mat;
212	return *this;
213}
214
215
216F32 LLMatrix4::determinant() const
217{
218	F32 value =
219	    mMatrix[0][3] * mMatrix[1][2] * mMatrix[2][1] * mMatrix[3][0] -
220	    mMatrix[0][2] * mMatrix[1][3] * mMatrix[2][1] * mMatrix[3][0] -
221	    mMatrix[0][3] * mMatrix[1][1] * mMatrix[2][2] * mMatrix[3][0] +
222	    mMatrix[0][1] * mMatrix[1][3] * mMatrix[2][2] * mMatrix[3][0] +
223	    mMatrix[0][2] * mMatrix[1][1] * mMatrix[2][3] * mMatrix[3][0] -
224	    mMatrix[0][1] * mMatrix[1][2] * mMatrix[2][3] * mMatrix[3][0] -
225	    mMatrix[0][3] * mMatrix[1][2] * mMatrix[2][0] * mMatrix[3][1] +
226	    mMatrix[0][2] * mMatrix[1][3] * mMatrix[2][0] * mMatrix[3][1] +
227	    mMatrix[0][3] * mMatrix[1][0] * mMatrix[2][2] * mMatrix[3][1] -
228	    mMatrix[0][0] * mMatrix[1][3] * mMatrix[2][2] * mMatrix[3][1] -
229	    mMatrix[0][2] * mMatrix[1][0] * mMatrix[2][3] * mMatrix[3][1] +
230	    mMatrix[0][0] * mMatrix[1][2] * mMatrix[2][3] * mMatrix[3][1] +
231	    mMatrix[0][3] * mMatrix[1][1] * mMatrix[2][0] * mMatrix[3][2] -
232	    mMatrix[0][1] * mMatrix[1][3] * mMatrix[2][0] * mMatrix[3][2] -
233	    mMatrix[0][3] * mMatrix[1][0] * mMatrix[2][1] * mMatrix[3][2] +
234	    mMatrix[0][0] * mMatrix[1][3] * mMatrix[2][1] * mMatrix[3][2] +
235	    mMatrix[0][1] * mMatrix[1][0] * mMatrix[2][3] * mMatrix[3][2] -
236	    mMatrix[0][0] * mMatrix[1][1] * mMatrix[2][3] * mMatrix[3][2] -
237	    mMatrix[0][2] * mMatrix[1][1] * mMatrix[2][0] * mMatrix[3][3] +
238	    mMatrix[0][1] * mMatrix[1][2] * mMatrix[2][0] * mMatrix[3][3] +
239	    mMatrix[0][2] * mMatrix[1][0] * mMatrix[2][1] * mMatrix[3][3] -
240	    mMatrix[0][0] * mMatrix[1][2] * mMatrix[2][1] * mMatrix[3][3] -
241	    mMatrix[0][1] * mMatrix[1][0] * mMatrix[2][2] * mMatrix[3][3] +
242		mMatrix[0][0] * mMatrix[1][1] * mMatrix[2][2] * mMatrix[3][3];
243
244	return value;
245}
246
247// Only works for pure orthonormal, homogeneous transform matrices.
248const LLMatrix4&	LLMatrix4::invert(void) 
249{
250	// transpose the rotation part
251	F32 temp;
252	temp = mMatrix[VX][VY]; mMatrix[VX][VY] = mMatrix[VY][VX]; mMatrix[VY][VX] = temp;
253	temp = mMatrix[VX][VZ]; mMatrix[VX][VZ] = mMatrix[VZ][VX]; mMatrix[VZ][VX] = temp;
254	temp = mMatrix[VY][VZ]; mMatrix[VY][VZ] = mMatrix[VZ][VY]; mMatrix[VZ][VY] = temp;
255
256	// rotate the translation part by the new rotation 
257	// (temporarily store in empty column of matrix)
258	U32 j;
259	for (j=0; j<3; j++)
260	{
261		mMatrix[j][VW] =  mMatrix[VW][VX] * mMatrix[VX][j] + 
262						  mMatrix[VW][VY] * mMatrix[VY][j] +
263						  mMatrix[VW][VZ] * mMatrix[VZ][j]; 
264	}
265
266	// negate and copy the temporary vector back to the tranlation row
267	mMatrix[VW][VX] = -mMatrix[VX][VW];
268	mMatrix[VW][VY] = -mMatrix[VY][VW];
269	mMatrix[VW][VZ] = -mMatrix[VZ][VW];
270
271   	// zero the empty column again
272	mMatrix[VX][VW] = mMatrix[VY][VW] = mMatrix[VZ][VW] = 0.0f;
273	
274	return *this;
275}
276
277LLVector4 LLMatrix4::getFwdRow4() const
278{
279	return LLVector4(mMatrix[VX][VX], mMatrix[VX][VY], mMatrix[VX][VZ], mMatrix[VX][VW]);
280}
281
282LLVector4 LLMatrix4::getLeftRow4() const
283{
284	return LLVector4(mMatrix[VY][VX], mMatrix[VY][VY], mMatrix[VY][VZ], mMatrix[VY][VW]);
285}
286
287LLVector4 LLMatrix4::getUpRow4() const
288{
289	return LLVector4(mMatrix[VZ][VX], mMatrix[VZ][VY], mMatrix[VZ][VZ], mMatrix[VZ][VW]);
290}
291
292// SJB: This code is correct for a logicly stored (non-transposed) matrix;
293//		Our matrices are stored transposed, OpenGL style, so this generates the
294//		INVERSE quaternion (-x, -y, -z, w)!
295//		Because we use similar logic in LLQuaternion::getMatrix3,
296//		we are internally consistant so everything works OK :)
297LLQuaternion	LLMatrix4::quaternion() const
298{
299	LLQuaternion	quat;
300	F32		tr, s, q[4];
301	U32		i, j, k;
302	U32		nxt[3] = {1, 2, 0};
303
304	tr = mMatrix[0][0] + mMatrix[1][1] + mMatrix[2][2];
305
306	// check the diagonal
307	if (tr > 0.f) 
308	{
309		s = (F32)sqrt (tr + 1.f);
310		quat.mQ[VS] = s / 2.f;
311		s = 0.5f / s;
312		quat.mQ[VX] = (mMatrix[1][2] - mMatrix[2][1]) * s;
313		quat.mQ[VY] = (mMatrix[2][0] - mMatrix[0][2]) * s;
314		quat.mQ[VZ] = (mMatrix[0][1] - mMatrix[1][0]) * s;
315	} 
316	else
317	{		
318		// diagonal is negative
319		i = 0;
320		if (mMatrix[1][1] > mMatrix[0][0]) 
321			i = 1;
322		if (mMatrix[2][2] > mMatrix[i][i]) 
323			i = 2;
324
325		j = nxt[i];
326		k = nxt[j];
327
328
329		s = (F32)sqrt ((mMatrix[i][i] - (mMatrix[j][j] + mMatrix[k][k])) + 1.f);
330
331		q[i] = s * 0.5f;
332
333		if (s != 0.f) 
334			s = 0.5f / s;
335
336		q[3] = (mMatrix[j][k] - mMatrix[k][j]) * s;
337		q[j] = (mMatrix[i][j] + mMatrix[j][i]) * s;
338		q[k] = (mMatrix[i][k] + mMatrix[k][i]) * s;
339
340		quat.setQuat(q);
341	}
342	return quat;
343}
344
345
346
347void LLMatrix4::initRows(const LLVector4 &row0,
348						 const LLVector4 &row1,
349						 const LLVector4 &row2,
350						 const LLVector4 &row3)
351{
352	mMatrix[0][0] = row0.mV[0];
353	mMatrix[0][1] = row0.mV[1];
354	mMatrix[0][2] = row0.mV[2];
355	mMatrix[0][3] = row0.mV[3];
356
357	mMatrix[1][0] = row1.mV[0];
358	mMatrix[1][1] = row1.mV[1];
359	mMatrix[1][2] = row1.mV[2];
360	mMatrix[1][3] = row1.mV[3];
361
362	mMatrix[2][0] = row2.mV[0];
363	mMatrix[2][1] = row2.mV[1];
364	mMatrix[2][2] = row2.mV[2];
365	mMatrix[2][3] = row2.mV[3];
366
367	mMatrix[3][0] = row3.mV[0];
368	mMatrix[3][1] = row3.mV[1];
369	mMatrix[3][2] = row3.mV[2];
370	mMatrix[3][3] = row3.mV[3];
371}
372
373
374const LLMatrix4& 	LLMatrix4::initRotation(const F32 angle, const F32 x, const F32 y, const F32 z)
375{
376	LLMatrix3	mat(angle, x, y, z);
377	return initMatrix(mat);
378}
379
380
381const LLMatrix4& 	LLMatrix4::initRotation(F32 angle, const LLVector4 &vec)
382{
383	LLMatrix3	mat(angle, vec);
384	return initMatrix(mat);
385}
386
387
388const LLMatrix4& 	LLMatrix4::initRotation(const F32 roll, const F32 pitch, const F32 yaw)
389{
390	LLMatrix3	mat(roll, pitch, yaw);
391	return initMatrix(mat);
392}
393
394
395const LLMatrix4&  	LLMatrix4::initRotation(const LLQuaternion &q)
396{
397	LLMatrix3	mat(q);
398	return initMatrix(mat);
399}
400
401
402// Position and Rotation
403const LLMatrix4&  	LLMatrix4::initRotTrans(const F32 angle, const F32 rx, const F32 ry, const F32 rz,
404											const F32 tx, const F32 ty, const F32 tz)
405{
406	LLMatrix3	mat(angle, rx, ry, rz);
407	LLVector3	translation(tx, ty, tz);
408	initMatrix(mat);
409	setTranslation(translation);
410	return (*this);
411}
412
413const LLMatrix4&  	LLMatrix4::initRotTrans(const F32 angle, const LLVector3 &axis, const LLVector3&translation)
414{
415	LLMatrix3	mat(angle, axis);
416	initMatrix(mat);
417	setTranslation(translation);
418	return (*this);
419}
420
421const LLMatrix4&  	LLMatrix4::initRotTrans(const F32 roll, const F32 pitch, const F32 yaw, const LLVector4 &translation)
422{
423	LLMatrix3	mat(roll, pitch, yaw);
424	initMatrix(mat);
425	setTranslation(translation);
426	return (*this);
427}
428
429/*
430const LLMatrix4&  	LLMatrix4::initRotTrans(const LLVector4 &fwd, 
431											const LLVector4 &left, 
432											const LLVector4 &up, 
433											const LLVector4 &translation)
434{
435	LLMatrix3 mat(fwd, left, up);
436	initMatrix(mat);
437	setTranslation(translation);
438	return (*this);
439}
440*/
441
442const LLMatrix4&  	LLMatrix4::initRotTrans(const LLQuaternion &q, const LLVector4 &translation)
443{
444	LLMatrix3	mat(q);
445	initMatrix(mat);
446	setTranslation(translation);
447	return (*this);
448}
449
450const LLMatrix4& LLMatrix4::initScale(const LLVector3 &scale)
451{
452	setIdentity();
453
454	mMatrix[VX][VX] = scale.mV[VX];
455	mMatrix[VY][VY] = scale.mV[VY];
456	mMatrix[VZ][VZ] = scale.mV[VZ];
457	
458	return (*this);
459}
460
461const LLMatrix4& LLMatrix4::initAll(const LLVector3 &scale, const LLQuaternion &q, const LLVector3 &pos)
462{
463	F32		sx, sy, sz;
464	F32		xx, xy, xz, xw, yy, yz, yw, zz, zw;
465
466	sx      = scale.mV[0];
467	sy      = scale.mV[1];
468	sz      = scale.mV[2];
469
470    xx      = q.mQ[VX] * q.mQ[VX];
471    xy      = q.mQ[VX] * q.mQ[VY];
472    xz      = q.mQ[VX] * q.mQ[VZ];
473    xw      = q.mQ[VX] * q.mQ[VW];
474
475    yy      = q.mQ[VY] * q.mQ[VY];
476    yz      = q.mQ[VY] * q.mQ[VZ];
477    yw      = q.mQ[VY] * q.mQ[VW];
478
479    zz      = q.mQ[VZ] * q.mQ[VZ];
480    zw      = q.mQ[VZ] * q.mQ[VW];
481
482    mMatrix[0][0]  = (1.f - 2.f * ( yy + zz )) *sx;
483    mMatrix[0][1]  = (	    2.f * ( xy + zw )) *sx;
484    mMatrix[0][2]  = (	    2.f * ( xz - yw )) *sx;
485
486    mMatrix[1][0]  = ( 	    2.f * ( xy - zw )) *sy;
487    mMatrix[1][1]  = (1.f - 2.f * ( xx + zz )) *sy;
488    mMatrix[1][2]  = (      2.f * ( yz + xw )) *sy;
489
490    mMatrix[2][0]  = (	    2.f * ( xz + yw )) *sz;
491    mMatrix[2][1]  = ( 	    2.f * ( yz - xw )) *sz;
492    mMatrix[2][2]  = (1.f - 2.f * ( xx + yy )) *sz;
493
494	mMatrix[3][0]  = pos.mV[0];
495	mMatrix[3][1]  = pos.mV[1];
496	mMatrix[3][2]  = pos.mV[2];
497	mMatrix[3][3]  = 1.0;
498
499	// TODO -- should we set the translation portion to zero?
500	return (*this);
501}
502
503// Rotate exisitng mMatrix
504const LLMatrix4&  	LLMatrix4::rotate(const F32 angle, const F32 x, const F32 y, const F32 z)
505{
506	LLVector4	vec4(x, y, z);
507	LLMatrix4	mat(angle, vec4);
508	*this *= mat;
509	return *this;
510}
511
512const LLMatrix4&  	LLMatrix4::rotate(const F32 angle, const LLVector4 &vec)
513{
514	LLMatrix4	mat(angle, vec);
515	*this *= mat;
516	return *this;
517}
518
519const LLMatrix4&  	LLMatrix4::rotate(const F32 roll, const F32 pitch, const F32 yaw)
520{
521	LLMatrix4	mat(roll, pitch, yaw);
522	*this *= mat;
523	return *this;
524}
525
526const LLMatrix4&  	LLMatrix4::rotate(const LLQuaternion &q)
527{
528	LLMatrix4	mat(q);
529	*this *= mat;
530	return *this;
531}
532
533
534const LLMatrix4&  	LLMatrix4::translate(const LLVector3 &vec)
535{
536	mMatrix[3][0] += vec.mV[0];
537	mMatrix[3][1] += vec.mV[1];
538	mMatrix[3][2] += vec.mV[2];
539	return (*this);
540}
541
542
543void LLMatrix4::setFwdRow(const LLVector3 &row)
544{
545	mMatrix[VX][VX] = row.mV[VX];
546	mMatrix[VX][VY] = row.mV[VY];
547	mMatrix[VX][VZ] = row.mV[VZ];
548}
549
550void LLMatrix4::setLeftRow(const LLVector3 &row)
551{
552	mMatrix[VY][VX] = row.mV[VX];
553	mMatrix[VY][VY] = row.mV[VY];
554	mMatrix[VY][VZ] = row.mV[VZ];
555}
556
557void LLMatrix4::setUpRow(const LLVector3 &row)
558{
559	mMatrix[VZ][VX] = row.mV[VX];
560	mMatrix[VZ][VY] = row.mV[VY];
561	mMatrix[VZ][VZ] = row.mV[VZ];
562}
563
564
565void LLMatrix4::setFwdCol(const LLVector3 &col)
566{
567	mMatrix[VX][VX] = col.mV[VX];
568	mMatrix[VY][VX] = col.mV[VY];
569	mMatrix[VZ][VX] = col.mV[VZ];
570}
571
572void LLMatrix4::setLeftCol(const LLVector3 &col)
573{
574	mMatrix[VX][VY] = col.mV[VX];
575	mMatrix[VY][VY] = col.mV[VY];
576	mMatrix[VZ][VY] = col.mV[VZ];
577}
578
579void LLMatrix4::setUpCol(const LLVector3 &col)
580{
581	mMatrix[VX][VZ] = col.mV[VX];
582	mMatrix[VY][VZ] = col.mV[VY];
583	mMatrix[VZ][VZ] = col.mV[VZ];
584}
585
586
587const LLMatrix4&  	LLMatrix4::setTranslation(const F32 tx, const F32 ty, const F32 tz)
588{
589	mMatrix[VW][VX] = tx;
590	mMatrix[VW][VY] = ty;
591	mMatrix[VW][VZ] = tz;
592	return (*this);
593}
594
595const LLMatrix4&  	LLMatrix4::setTranslation(const LLVector3 &translation)
596{
597	mMatrix[VW][VX] = translation.mV[VX];
598	mMatrix[VW][VY] = translation.mV[VY];
599	mMatrix[VW][VZ] = translation.mV[VZ];
600	return (*this);
601}
602
603const LLMatrix4&  	LLMatrix4::setTranslation(const LLVector4 &translation)
604{
605	mMatrix[VW][VX] = translation.mV[VX];
606	mMatrix[VW][VY] = translation.mV[VY];
607	mMatrix[VW][VZ] = translation.mV[VZ];
608	return (*this);
609}
610
611// LLMatrix3 Extraction and Setting
612LLMatrix3  	LLMatrix4::getMat3() const
613{
614	LLMatrix3 retmat;
615
616	retmat.mMatrix[0][0] = mMatrix[0][0];
617	retmat.mMatrix[0][1] = mMatrix[0][1];
618	retmat.mMatrix[0][2] = mMatrix[0][2];
619
620	retmat.mMatrix[1][0] = mMatrix[1][0];
621	retmat.mMatrix[1][1] = mMatrix[1][1];
622	retmat.mMatrix[1][2] = mMatrix[1][2];
623
624	retmat.mMatrix[2][0] = mMatrix[2][0];
625	retmat.mMatrix[2][1] = mMatrix[2][1];
626	retmat.mMatrix[2][2] = mMatrix[2][2];
627
628	return retmat;
629}
630
631const LLMatrix4&  	LLMatrix4::initMatrix(const LLMatrix3 &mat)
632{
633	mMatrix[0][0] = mat.mMatrix[0][0];
634	mMatrix[0][1] = mat.mMatrix[0][1];
635	mMatrix[0][2] = mat.mMatrix[0][2];
636	mMatrix[0][3] = 0.f;
637
638	mMatrix[1][0] = mat.mMatrix[1][0];
639	mMatrix[1][1] = mat.mMatrix[1][1];
640	mMatrix[1][2] = mat.mMatrix[1][2];
641	mMatrix[1][3] = 0.f;
642
643	mMatrix[2][0] = mat.mMatrix[2][0];
644	mMatrix[2][1] = mat.mMatrix[2][1];
645	mMatrix[2][2] = mat.mMatrix[2][2];
646	mMatrix[2][3] = 0.f;
647
648	mMatrix[3][0] = 0.f;
649	mMatrix[3][1] = 0.f;
650	mMatrix[3][2] = 0.f;
651	mMatrix[3][3] = 1.f;
652	return (*this);
653}
654
655const LLMatrix4&  	LLMatrix4::initMatrix(const LLMatrix3 &mat, const LLVector4 &translation)
656{
657	mMatrix[0][0] = mat.mMatrix[0][0];
658	mMatrix[0][1] = mat.mMatrix[0][1];
659	mMatrix[0][2] = mat.mMatrix[0][2];
660	mMatrix[0][3] = 0.f;
661
662	mMatrix[1][0] = mat.mMatrix[1][0];
663	mMatrix[1][1] = mat.mMatrix[1][1];
664	mMatrix[1][2] = mat.mMatrix[1][2];
665	mMatrix[1][3] = 0.f;
666
667	mMatrix[2][0] = mat.mMatrix[2][0];
668	mMatrix[2][1] = mat.mMatrix[2][1];
669	mMatrix[2][2] = mat.mMatrix[2][2];
670	mMatrix[2][3] = 0.f;
671
672	mMatrix[3][0] = translation.mV[0];
673	mMatrix[3][1] = translation.mV[1];
674	mMatrix[3][2] = translation.mV[2];
675	mMatrix[3][3] = 1.f;
676	return (*this);
677}
678
679// LLMatrix4 Operators
680
681LLVector4 operator*(const LLVector4 &a, const LLMatrix4 &b)
682{
683	// Operate "to the left" on row-vector a
684	return LLVector4(a.mV[VX] * b.mMatrix[VX][VX] + 
685					 a.mV[VY] * b.mMatrix[VY][VX] + 
686					 a.mV[VZ] * b.mMatrix[VZ][VX] +
687					 a.mV[VW] * b.mMatrix[VW][VX],
688
689					 a.mV[VX] * b.mMatrix[VX][VY] + 
690					 a.mV[VY] * b.mMatrix[VY][VY] + 
691					 a.mV[VZ] * b.mMatrix[VZ][VY] +
692					 a.mV[VW] * b.mMatrix[VW][VY],
693
694					 a.mV[VX] * b.mMatrix[VX][VZ] + 
695					 a.mV[VY] * b.mMatrix[VY][VZ] + 
696					 a.mV[VZ] * b.mMatrix[VZ][VZ] +
697					 a.mV[VW] * b.mMatrix[VW][VZ],
698
699					 a.mV[VX] * b.mMatrix[VX][VW] + 
700					 a.mV[VY] * b.mMatrix[VY][VW] + 
701					 a.mV[VZ] * b.mMatrix[VZ][VW] +
702					 a.mV[VW] * b.mMatrix[VW][VW]);
703}
704
705LLVector4 rotate_vector(const LLVector4 &a, const LLMatrix4 &b)
706{
707	// Rotates but does not translate
708	// Operate "to the left" on row-vector a
709	LLVector4	vec;
710	vec.mV[VX] = a.mV[VX] * b.mMatrix[VX][VX] + 
711				 a.mV[VY] * b.mMatrix[VY][VX] + 
712				 a.mV[VZ] * b.mMatrix[VZ][VX];
713
714	vec.mV[VY] = a.mV[VX] * b.mMatrix[VX][VY] + 
715				 a.mV[VY] * b.mMatrix[VY][VY] + 
716				 a.mV[VZ] * b.mMatrix[VZ][VY];
717
718	vec.mV[VZ] = a.mV[VX] * b.mMatrix[VX][VZ] + 
719				 a.mV[VY] * b.mMatrix[VY][VZ] + 
720				 a.mV[VZ] * b.mMatrix[VZ][VZ];
721
722//	vec.mV[VW] = a.mV[VX] * b.mMatrix[VX][VW] + 
723//				 a.mV[VY] * b.mMatrix[VY][VW] + 
724//				 a.mV[VZ] * b.mMatrix[VZ][VW] +
725	vec.mV[VW] = a.mV[VW];
726	return vec;
727}
728
729LLVector3 rotate_vector(const LLVector3 &a, const LLMatrix4 &b)
730{
731	// Rotates but does not translate
732	// Operate "to the left" on row-vector a
733	LLVector3	vec;
734	vec.mV[VX] = a.mV[VX] * b.mMatrix[VX][VX] + 
735				 a.mV[VY] * b.mMatrix[VY][VX] + 
736				 a.mV[VZ] * b.mMatrix[VZ][VX];
737
738	vec.mV[VY] = a.mV[VX] * b.mMatrix[VX][VY] + 
739				 a.mV[VY] * b.mMatrix[VY][VY] + 
740				 a.mV[VZ] * b.mMatrix[VZ][VY];
741
742	vec.mV[VZ] = a.mV[VX] * b.mMatrix[VX][VZ] + 
743				 a.mV[VY] * b.mMatrix[VY][VZ] + 
744				 a.mV[VZ] * b.mMatrix[VZ][VZ];
745	return vec;
746}
747
748bool operator==(const LLMatrix4 &a, const LLMatrix4 &b)
749{
750	U32		i, j;
751	for (i = 0; i < NUM_VALUES_IN_MAT4; i++)
752	{
753		for (j = 0; j < NUM_VALUES_IN_MAT4; j++)
754		{
755			if (a.mMatrix[j][i] != b.mMatrix[j][i])
756				return FALSE;
757		}
758	}
759	return TRUE;
760}
761
762bool operator!=(const LLMatrix4 &a, const LLMatrix4 &b)
763{
764	U32		i, j;
765	for (i = 0; i < NUM_VALUES_IN_MAT4; i++)
766	{
767		for (j = 0; j < NUM_VALUES_IN_MAT4; j++)
768		{
769			if (a.mMatrix[j][i] != b.mMatrix[j][i])
770				return TRUE;
771		}
772	}
773	return FALSE;
774}
775
776bool operator<(const LLMatrix4& a, const LLMatrix4 &b)
777{
778	U32		i, j;
779	for (i = 0; i < NUM_VALUES_IN_MAT4; i++)
780	{
781		for (j = 0; j < NUM_VALUES_IN_MAT4; j++)
782		{
783			if (a.mMatrix[i][j] != b.mMatrix[i][j])
784			{
785				return a.mMatrix[i][j] < b.mMatrix[i][j];
786			}
787		}
788	}
789
790	return false;
791}
792
793const LLMatrix4& operator*=(LLMatrix4 &a, F32 k)
794{
795	U32		i, j;
796	for (i = 0; i < NUM_VALUES_IN_MAT4; i++)
797	{
798		for (j = 0; j < NUM_VALUES_IN_MAT4; j++)
799		{
800			a.mMatrix[j][i] *= k;
801		}
802	}
803	return a;
804}
805
806std::ostream& operator<<(std::ostream& s, const LLMatrix4 &a) 
807{
808	s << "{ " 
809		<< a.mMatrix[VX][VX] << ", " 
810		<< a.mMatrix[VX][VY] << ", " 
811		<< a.mMatrix[VX][VZ] << ", " 
812		<< a.mMatrix[VX][VW] 
813		<< "; "
814		<< a.mMatrix[VY][VX] << ", " 
815		<< a.mMatrix[VY][VY] << ", " 
816		<< a.mMatrix[VY][VZ] << ", " 
817		<< a.mMatrix[VY][VW] 
818		<< "; "
819		<< a.mMatrix[VZ][VX] << ", " 
820		<< a.mMatrix[VZ][VY] << ", " 
821		<< a.mMatrix[VZ][VZ] << ", " 
822		<< a.mMatrix[VZ][VW] 
823		<< "; "
824		<< a.mMatrix[VW][VX] << ", " 
825		<< a.mMatrix[VW][VY] << ", " 
826		<< a.mMatrix[VW][VZ] << ", " 
827		<< a.mMatrix[VW][VW] 
828	  << " }";
829	return s;
830}
831
832LLSD LLMatrix4::getValue() const
833{
834	LLSD ret;
835	
836	ret[0] = mMatrix[0][0];
837	ret[1] = mMatrix[0][1];
838	ret[2] = mMatrix[0][2];
839	ret[3] = mMatrix[0][3];
840
841	ret[4] = mMatrix[1][0];
842	ret[5] = mMatrix[1][1];
843	ret[6] = mMatrix[1][2];
844	ret[7] = mMatrix[1][3];
845
846	ret[8] = mMatrix[2][0];
847	ret[9] = mMatrix[2][1];
848	ret[10] = mMatrix[2][2];
849	ret[11] = mMatrix[2][3];
850
851	ret[12] = mMatrix[3][0];
852	ret[13] = mMatrix[3][1];
853	ret[14] = mMatrix[3][2];
854	ret[15] = mMatrix[3][3];
855
856	return ret;
857}
858
859void LLMatrix4::setValue(const LLSD& data) 
860{
861	mMatrix[0][0] = data[0].asReal();
862	mMatrix[0][1] = data[1].asReal();
863	mMatrix[0][2] = data[2].asReal();
864	mMatrix[0][3] = data[3].asReal();
865
866	mMatrix[1][0] = data[4].asReal();
867	mMatrix[1][1] = data[5].asReal();
868	mMatrix[1][2] = data[6].asReal();
869	mMatrix[1][3] = data[7].asReal();
870
871	mMatrix[2][0] = data[8].asReal();
872	mMatrix[2][1] = data[9].asReal();
873	mMatrix[2][2] = data[10].asReal();
874	mMatrix[2][3] = data[11].asReal();
875
876	mMatrix[3][0] = data[12].asReal();
877	mMatrix[3][1] = data[13].asReal();
878	mMatrix[3][2] = data[14].asReal();
879	mMatrix[3][3] = data[15].asReal();
880}
881
882