PageRenderTime 56ms CodeModel.GetById 2ms app.highlight 49ms RepoModel.GetById 1ms app.codeStats 1ms

/indra/llmath/llcoordframe.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 778 lines | 555 code | 136 blank | 87 comment | 32 complexity | ae4a122f1d56c57d13b7dfa76250c06b MD5 | raw file
  1/** 
  2 * @file llcoordframe.cpp
  3 * @brief LLCoordFrame 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 "m3math.h"
 32#include "v4math.h"
 33#include "m4math.h"
 34#include "llquaternion.h"
 35#include "llcoordframe.h"
 36
 37#ifndef X_AXIS
 38	#define X_AXIS 1.0f,0.0f,0.0f
 39	#define Y_AXIS 0.0f,1.0f,0.0f
 40	#define Z_AXIS 0.0f,0.0f,1.0f
 41#endif
 42
 43// Constructors
 44
 45LLCoordFrame::LLCoordFrame() :
 46	mOrigin(0.f, 0.f, 0.f),
 47	mXAxis(X_AXIS),
 48	mYAxis(Y_AXIS),
 49	mZAxis(Z_AXIS)
 50{
 51}
 52
 53LLCoordFrame::LLCoordFrame(const LLVector3 &origin) :
 54	mOrigin(origin), 
 55	mXAxis(X_AXIS),
 56	mYAxis(Y_AXIS),
 57	mZAxis(Z_AXIS)
 58{
 59	if( !mOrigin.isFinite() )
 60	{
 61		reset();
 62		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
 63	}
 64}
 65
 66LLCoordFrame::LLCoordFrame(const LLVector3 &origin, const LLVector3 &direction) :
 67	mOrigin(origin)
 68{
 69	lookDir(direction);
 70	
 71	if( !isFinite() )
 72	{
 73		reset();
 74		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
 75	}
 76}
 77
 78LLCoordFrame::LLCoordFrame(const LLVector3 &x_axis,
 79						   const LLVector3 &y_axis,
 80						   const LLVector3 &z_axis) : 
 81	mOrigin(0.f, 0.f, 0.f), 
 82	mXAxis(x_axis), 
 83	mYAxis(y_axis), 
 84	mZAxis(z_axis)
 85{
 86	if( !isFinite() )
 87	{
 88		reset();
 89		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
 90	}
 91}
 92
 93LLCoordFrame::LLCoordFrame(const LLVector3 &origin,
 94						   const LLVector3 &x_axis,
 95						   const LLVector3 &y_axis,
 96						   const LLVector3 &z_axis) : 
 97	mOrigin(origin), 
 98	mXAxis(x_axis), 
 99	mYAxis(y_axis), 
100	mZAxis(z_axis)
101{
102	if( !isFinite() )
103	{
104		reset();
105		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
106	}
107}
108
109
110LLCoordFrame::LLCoordFrame(const LLVector3 &origin, 
111						   const LLMatrix3 &rotation) :
112	mOrigin(origin),
113	mXAxis(rotation.mMatrix[VX]),
114	mYAxis(rotation.mMatrix[VY]),
115	mZAxis(rotation.mMatrix[VZ])
116{
117	if( !isFinite() )
118	{
119		reset();
120		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
121	}
122}
123
124LLCoordFrame::LLCoordFrame(const LLQuaternion &q) :
125	mOrigin(0.f, 0.f, 0.f)
126{
127	LLMatrix3 rotation_matrix(q);
128	mXAxis.setVec(rotation_matrix.mMatrix[VX]);
129	mYAxis.setVec(rotation_matrix.mMatrix[VY]);
130	mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
131
132	if( !isFinite() )
133	{
134		reset();
135		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
136	}
137}
138
139LLCoordFrame::LLCoordFrame(const LLVector3 &origin, const LLQuaternion &q) :
140	mOrigin(origin)
141{
142	LLMatrix3 rotation_matrix(q);
143	mXAxis.setVec(rotation_matrix.mMatrix[VX]);
144	mYAxis.setVec(rotation_matrix.mMatrix[VY]);
145	mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
146
147	if( !isFinite() )
148	{
149		reset();
150		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
151	}
152}
153
154LLCoordFrame::LLCoordFrame(const LLMatrix4 &mat) :
155	mOrigin(mat.mMatrix[VW]),
156	mXAxis(mat.mMatrix[VX]),
157	mYAxis(mat.mMatrix[VY]),
158	mZAxis(mat.mMatrix[VZ])
159{
160	if( !isFinite() )
161	{
162		reset();
163		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
164	}
165}
166
167
168// The folowing two constructors are dangerous due to implicit casting and have been disabled - SJB
169/*
170LLCoordFrame::LLCoordFrame(const F32 *origin, const F32 *rotation) :
171	mOrigin(origin),
172	mXAxis(rotation+3*VX),
173	mYAxis(rotation+3*VY),
174	mZAxis(rotation+3*VZ)
175{
176	if( !isFinite() )
177	{
178		reset();
179		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
180	}
181}
182*/
183
184/*
185LLCoordFrame::LLCoordFrame(const F32 *origin_and_rotation) :
186	mOrigin(origin_and_rotation),
187	mXAxis(origin_and_rotation + 3*(VX+1)),
188	mYAxis(origin_and_rotation + 3*(VY+1)),
189	mZAxis(origin_and_rotation + 3*(VZ+1))
190{
191	if( !isFinite() )
192	{
193		reset();
194		llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
195	}
196}
197*/
198
199
200void LLCoordFrame::reset() 
201{
202	mOrigin.setVec(0.0f, 0.0f, 0.0f);
203	resetAxes();
204}
205
206
207void LLCoordFrame::resetAxes()
208{
209	mXAxis.setVec(1.0f, 0.0f, 0.0f);
210	mYAxis.setVec(0.0f, 1.0f, 0.0f);
211	mZAxis.setVec(0.0f, 0.0f, 1.0f);
212}
213
214// setOrigin() member functions set mOrigin
215
216void LLCoordFrame::setOrigin(F32 x, F32 y, F32 z) 
217{
218	mOrigin.setVec(x, y, z); 
219
220	if( !mOrigin.isFinite() )
221	{
222		reset();
223		llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
224	}
225}
226
227void LLCoordFrame::setOrigin(const LLVector3 &new_origin)
228{
229	mOrigin = new_origin; 
230	if( !mOrigin.isFinite() )
231	{
232		reset();
233		llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
234	}
235}
236
237void LLCoordFrame::setOrigin(const F32 *origin)
238{
239	mOrigin.mV[VX] = *(origin + VX);
240	mOrigin.mV[VY] = *(origin + VY);
241	mOrigin.mV[VZ] = *(origin + VZ);
242
243	if( !mOrigin.isFinite() )
244	{
245		reset();
246		llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
247	}
248}
249
250void LLCoordFrame::setOrigin(const LLCoordFrame &frame)
251{
252	mOrigin = frame.getOrigin();
253
254	if( !mOrigin.isFinite() )
255	{
256		reset();
257		llwarns << "Non Finite in LLCoordFrame::setOrigin()" << llendl;
258	}
259}
260
261// setAxes()  member functions set the axes, and assume that
262// the arguments are orthogonal and normalized.
263
264void LLCoordFrame::setAxes(const LLVector3 &x_axis,
265						  const LLVector3 &y_axis,
266						  const LLVector3 &z_axis)
267{
268	mXAxis = x_axis;
269	mYAxis = y_axis;
270	mZAxis = z_axis;
271	if( !isFinite() )
272	{
273		reset();
274		llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
275	}
276}
277
278
279void LLCoordFrame::setAxes(const LLMatrix3 &rotation_matrix)
280{
281	mXAxis.setVec(rotation_matrix.mMatrix[VX]);
282	mYAxis.setVec(rotation_matrix.mMatrix[VY]);
283	mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
284	if( !isFinite() )
285	{
286		reset();
287		llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
288	}
289}
290
291
292void LLCoordFrame::setAxes(const LLQuaternion &q )
293{
294	LLMatrix3 rotation_matrix(q);
295	setAxes(rotation_matrix);
296	if( !isFinite() )
297	{
298		reset();
299		llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
300	}
301}
302
303
304void LLCoordFrame::setAxes(  const F32 *rotation_matrix ) 
305{
306	mXAxis.mV[VX] = *(rotation_matrix + 3*VX + VX);
307	mXAxis.mV[VY] = *(rotation_matrix + 3*VX + VY);
308	mXAxis.mV[VZ] = *(rotation_matrix + 3*VX + VZ);
309	mYAxis.mV[VX] = *(rotation_matrix + 3*VY + VX);
310	mYAxis.mV[VY] = *(rotation_matrix + 3*VY + VY);
311	mYAxis.mV[VZ] = *(rotation_matrix + 3*VY + VZ);
312	mZAxis.mV[VX] = *(rotation_matrix + 3*VZ + VX);
313	mZAxis.mV[VY] = *(rotation_matrix + 3*VZ + VY);
314	mZAxis.mV[VZ] = *(rotation_matrix + 3*VZ + VZ);
315
316	if( !isFinite() )
317	{
318		reset();
319		llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
320	}
321}
322
323
324void LLCoordFrame::setAxes(const LLCoordFrame &frame)
325{
326	mXAxis = frame.getXAxis();
327	mYAxis = frame.getYAxis();
328	mZAxis = frame.getZAxis();
329
330	if( !isFinite() )
331	{
332		reset();
333		llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
334	}
335}
336
337
338// translate() member functions move mOrigin to a relative position
339
340void LLCoordFrame::translate(F32 x, F32 y, F32 z)
341{
342	mOrigin.mV[VX] += x;
343	mOrigin.mV[VY] += y;
344	mOrigin.mV[VZ] += z;
345
346	if( !mOrigin.isFinite() )
347	{
348		reset();
349		llwarns << "Non Finite in LLCoordFrame::translate()" << llendl;
350	}
351}
352
353
354void LLCoordFrame::translate(const LLVector3 &v)
355{
356	mOrigin += v;
357
358	if( !mOrigin.isFinite() )
359	{
360		reset();
361		llwarns << "Non Finite in LLCoordFrame::translate()" << llendl;
362	}
363}
364
365
366void LLCoordFrame::translate(const F32 *origin)
367{
368	mOrigin.mV[VX] += *(origin + VX);
369	mOrigin.mV[VY] += *(origin + VY);
370	mOrigin.mV[VZ] += *(origin + VZ);
371
372	if( !mOrigin.isFinite() )
373	{
374		reset();
375		llwarns << "Non Finite in LLCoordFrame::translate()" << llendl;
376	}
377}
378
379
380// Rotate move the axes to a relative rotation
381
382void LLCoordFrame::rotate(F32 angle, F32 x, F32 y, F32 z)
383{
384	LLQuaternion q(angle, LLVector3(x,y,z));
385	rotate(q);
386}
387
388
389void LLCoordFrame::rotate(F32 angle, const LLVector3 &rotation_axis)
390{
391	LLQuaternion q(angle, rotation_axis);
392	rotate(q);
393}
394
395
396void LLCoordFrame::rotate(const LLQuaternion &q)
397{
398	LLMatrix3 rotation_matrix(q);
399	rotate(rotation_matrix);
400}
401
402
403void LLCoordFrame::rotate(const LLMatrix3 &rotation_matrix)
404{
405	mXAxis.rotVec(rotation_matrix);
406	mYAxis.rotVec(rotation_matrix);
407	orthonormalize();
408
409	if( !isFinite() )
410	{
411		reset();
412		llwarns << "Non Finite in LLCoordFrame::rotate()" << llendl;
413	}
414}
415
416
417void LLCoordFrame::roll(F32 angle)
418{
419	LLQuaternion q(angle, mXAxis);
420	LLMatrix3 rotation_matrix(q);
421	rotate(rotation_matrix);
422
423	if( !mYAxis.isFinite() || !mZAxis.isFinite() )
424	{
425		reset();
426		llwarns << "Non Finite in LLCoordFrame::roll()" << llendl;
427	}
428}
429
430void LLCoordFrame::pitch(F32 angle)
431{
432	LLQuaternion q(angle, mYAxis);
433	LLMatrix3 rotation_matrix(q);
434	rotate(rotation_matrix);
435
436	if( !mXAxis.isFinite() || !mZAxis.isFinite() )
437	{
438		reset();
439		llwarns << "Non Finite in LLCoordFrame::pitch()" << llendl;
440	}
441}
442
443void LLCoordFrame::yaw(F32 angle)
444{
445	LLQuaternion q(angle, mZAxis);
446	LLMatrix3 rotation_matrix(q);
447	rotate(rotation_matrix);
448
449	if( !mXAxis.isFinite() || !mYAxis.isFinite() )
450	{
451		reset();
452		llwarns << "Non Finite in LLCoordFrame::yaw()" << llendl;
453	}
454}
455
456// get*() routines
457
458
459LLQuaternion LLCoordFrame::getQuaternion() const
460{
461	LLQuaternion quat(mXAxis, mYAxis, mZAxis);
462	return quat;
463}
464
465
466void LLCoordFrame::getMatrixToLocal(LLMatrix4& mat) const
467{
468	mat.setFwdCol(mXAxis);
469	mat.setLeftCol(mYAxis);
470	mat.setUpCol(mZAxis);
471
472	mat.mMatrix[3][0] = -(mOrigin * LLVector3(mat.mMatrix[0][0], mat.mMatrix[1][0], mat.mMatrix[2][0]));
473	mat.mMatrix[3][1] = -(mOrigin * LLVector3(mat.mMatrix[0][1], mat.mMatrix[1][1], mat.mMatrix[2][1]));
474	mat.mMatrix[3][2] = -(mOrigin * LLVector3(mat.mMatrix[0][2], mat.mMatrix[1][2], mat.mMatrix[2][2]));
475}
476
477
478void LLCoordFrame::getRotMatrixToParent(LLMatrix4& mat) const
479{
480	// Note: moves into CFR
481	mat.setFwdRow(	-mYAxis );
482	mat.setLeftRow(	 mZAxis );
483	mat.setUpRow(	-mXAxis );
484}
485
486size_t LLCoordFrame::writeOrientation(char *buffer) const
487{
488	memcpy(buffer, mOrigin.mV, 3*sizeof(F32)); /*Flawfinder: ignore */
489	buffer += 3*sizeof(F32);
490	memcpy(buffer, mXAxis.mV, 3*sizeof(F32)); /*Flawfinder: ignore */
491	buffer += 3*sizeof(F32);
492	memcpy(buffer, mYAxis.mV, 3*sizeof(F32));/*Flawfinder: ignore */
493	buffer += 3*sizeof(F32);
494	memcpy(buffer, mZAxis.mV, 3*sizeof(F32));	/*Flawfinder: ignore */
495	return 12*sizeof(F32);
496}
497
498
499size_t LLCoordFrame::readOrientation(const char *buffer)
500{
501	memcpy(mOrigin.mV, buffer, 3*sizeof(F32));	/*Flawfinder: ignore */
502	buffer += 3*sizeof(F32);
503	memcpy(mXAxis.mV, buffer, 3*sizeof(F32));	/*Flawfinder: ignore */
504	buffer += 3*sizeof(F32);
505	memcpy(mYAxis.mV, buffer, 3*sizeof(F32));	/*Flawfinder: ignore */
506	buffer += 3*sizeof(F32);
507	memcpy(mZAxis.mV, buffer, 3*sizeof(F32));	/*Flawfinder: ignore */
508
509	if( !isFinite() )
510	{
511		reset();
512		llwarns << "Non Finite in LLCoordFrame::readOrientation()" << llendl;
513	}
514
515	return 12*sizeof(F32);
516}
517
518
519// rotation and transform vectors between reference frames
520
521LLVector3 LLCoordFrame::rotateToLocal(const LLVector3 &absolute_vector) const
522{
523	LLVector3 local_vector(mXAxis * absolute_vector,
524						   mYAxis * absolute_vector,
525						   mZAxis * absolute_vector);
526	return local_vector;
527}
528
529
530LLVector4 LLCoordFrame::rotateToLocal(const LLVector4 &absolute_vector) const
531{
532	LLVector4 local_vector;
533	local_vector.mV[VX] = mXAxis.mV[VX] * absolute_vector.mV[VX] +
534						  mXAxis.mV[VY] * absolute_vector.mV[VY] +
535						  mXAxis.mV[VZ] * absolute_vector.mV[VZ];
536	local_vector.mV[VY] = mYAxis.mV[VX] * absolute_vector.mV[VX] +
537						  mYAxis.mV[VY] * absolute_vector.mV[VY] +
538						  mYAxis.mV[VZ] * absolute_vector.mV[VZ];
539	local_vector.mV[VZ] = mZAxis.mV[VX] * absolute_vector.mV[VX] +
540						  mZAxis.mV[VY] * absolute_vector.mV[VY] +
541						  mZAxis.mV[VZ] * absolute_vector.mV[VZ];
542	local_vector.mV[VW] = absolute_vector.mV[VW];
543	return local_vector;
544}
545
546
547LLVector3 LLCoordFrame::rotateToAbsolute(const LLVector3 &local_vector) const
548{
549	LLVector3 absolute_vector;
550	absolute_vector.mV[VX] = mXAxis.mV[VX] * local_vector.mV[VX] +
551							 mYAxis.mV[VX] * local_vector.mV[VY] +
552							 mZAxis.mV[VX] * local_vector.mV[VZ];
553	absolute_vector.mV[VY] = mXAxis.mV[VY] * local_vector.mV[VX] +
554							 mYAxis.mV[VY] * local_vector.mV[VY] +
555							 mZAxis.mV[VY] * local_vector.mV[VZ];
556	absolute_vector.mV[VZ] = mXAxis.mV[VZ] * local_vector.mV[VX] +
557							 mYAxis.mV[VZ] * local_vector.mV[VY] +
558							 mZAxis.mV[VZ] * local_vector.mV[VZ];
559	return absolute_vector;
560}
561
562
563LLVector4 LLCoordFrame::rotateToAbsolute(const LLVector4 &local_vector) const
564{
565	LLVector4 absolute_vector;
566	absolute_vector.mV[VX] = mXAxis.mV[VX] * local_vector.mV[VX] +
567							 mYAxis.mV[VX] * local_vector.mV[VY] +
568							 mZAxis.mV[VX] * local_vector.mV[VZ];
569	absolute_vector.mV[VY] = mXAxis.mV[VY] * local_vector.mV[VX] +
570							 mYAxis.mV[VY] * local_vector.mV[VY] +
571							 mZAxis.mV[VY] * local_vector.mV[VZ];
572	absolute_vector.mV[VZ] = mXAxis.mV[VZ] * local_vector.mV[VX] +
573							 mYAxis.mV[VZ] * local_vector.mV[VY] +
574							 mZAxis.mV[VZ] * local_vector.mV[VZ];
575	absolute_vector.mV[VW] = local_vector[VW];
576	return absolute_vector;
577}
578
579
580void LLCoordFrame::orthonormalize()
581// Makes sure the axes are orthogonal and normalized.
582{
583	mXAxis.normVec();						// X is renormalized
584	mYAxis -= mXAxis * (mXAxis * mYAxis);	// Y remains in X-Y plane
585	mYAxis.normVec();						// Y is normalized
586	mZAxis = mXAxis % mYAxis;				// Z = X cross Y
587}
588
589
590LLVector3 LLCoordFrame::transformToLocal(const LLVector3 &absolute_vector) const
591{
592	return rotateToLocal(absolute_vector - mOrigin);
593}
594
595
596LLVector4 LLCoordFrame::transformToLocal(const LLVector4 &absolute_vector) const
597{
598	LLVector4 local_vector(absolute_vector);
599	local_vector.mV[VX] -= mOrigin.mV[VX];
600	local_vector.mV[VY] -= mOrigin.mV[VY];
601	local_vector.mV[VZ] -= mOrigin.mV[VZ];
602	return rotateToLocal(local_vector);
603}
604
605
606LLVector3 LLCoordFrame::transformToAbsolute(const LLVector3 &local_vector) const
607{
608	return (rotateToAbsolute(local_vector) + mOrigin);
609}
610
611
612LLVector4 LLCoordFrame::transformToAbsolute(const LLVector4 &local_vector) const
613{
614	LLVector4 absolute_vector;
615	absolute_vector = rotateToAbsolute(local_vector);
616	absolute_vector.mV[VX] += mOrigin.mV[VX];
617	absolute_vector.mV[VY] += mOrigin.mV[VY];
618	absolute_vector.mV[VZ] += mOrigin.mV[VZ];
619	return absolute_vector;
620}
621
622
623// This is how you combine a translation and rotation of a 
624// coordinate frame to get an OpenGL transformation matrix:
625//
626//     translation   *   rotation      =          transformation matrix
627//
628//     (i)->
629// (j)| 1  0  0  0 |   | a  d  g  0 |     |     a            d            g          0 |
630//  | | 0  1  0  0 | * | b  e  h  0 |  =  |     b            e            h          0 |
631//  V | 0  0  1  0 |   | c  f  i  0 |     |     c            f            i          0 |
632//    |-x -y -z  1 |   | 0  0  0  1 |     |-(ax+by+cz)  -(dx+ey+fz)  -(gx+hy+iz)     1 |
633//
634// where {a,b,c} = x-axis 
635//       {d,e,f} = y-axis 
636//       {g,h,i} = z-axis 
637//       {x,y,z} = origin
638
639void LLCoordFrame::getOpenGLTranslation(F32 *ogl_matrix) const
640{
641	*(ogl_matrix + 0)  = 1.0f;
642	*(ogl_matrix + 1)  = 0.0f;
643	*(ogl_matrix + 2)  = 0.0f;
644	*(ogl_matrix + 3)  = 0.0f;
645
646	*(ogl_matrix + 4)  = 0.0f;
647	*(ogl_matrix + 5)  = 1.0f;
648	*(ogl_matrix + 6)  = 0.0f;
649	*(ogl_matrix + 7)  = 0.0f;
650
651	*(ogl_matrix + 8)  = 0.0f;
652	*(ogl_matrix + 9)  = 0.0f;
653	*(ogl_matrix + 10) = 1.0f;
654	*(ogl_matrix + 11) = 0.0f;
655
656	*(ogl_matrix + 12) = -mOrigin.mV[VX];
657	*(ogl_matrix + 13) = -mOrigin.mV[VY];
658	*(ogl_matrix + 14) = -mOrigin.mV[VZ];
659	*(ogl_matrix + 15) = 1.0f;
660}
661
662
663void LLCoordFrame::getOpenGLRotation(F32 *ogl_matrix) const
664{
665	*(ogl_matrix + 0)  = mXAxis.mV[VX];
666	*(ogl_matrix + 4)  = mXAxis.mV[VY];
667	*(ogl_matrix + 8)  = mXAxis.mV[VZ];
668
669	*(ogl_matrix + 1)  = mYAxis.mV[VX];
670	*(ogl_matrix + 5)  = mYAxis.mV[VY];
671	*(ogl_matrix + 9)  = mYAxis.mV[VZ];
672
673	*(ogl_matrix + 2)  = mZAxis.mV[VX];
674	*(ogl_matrix + 6)  = mZAxis.mV[VY];
675	*(ogl_matrix + 10) = mZAxis.mV[VZ];
676
677	*(ogl_matrix + 3)  = 0.0f;
678	*(ogl_matrix + 7)  = 0.0f;
679	*(ogl_matrix + 11) = 0.0f;
680
681	*(ogl_matrix + 12) = 0.0f;
682	*(ogl_matrix + 13) = 0.0f;
683	*(ogl_matrix + 14) = 0.0f;
684	*(ogl_matrix + 15) = 1.0f;
685}
686
687
688void LLCoordFrame::getOpenGLTransform(F32 *ogl_matrix) const
689{
690	*(ogl_matrix + 0)  = mXAxis.mV[VX];
691	*(ogl_matrix + 4)  = mXAxis.mV[VY];
692	*(ogl_matrix + 8)  = mXAxis.mV[VZ];
693	*(ogl_matrix + 12) = -mOrigin * mXAxis;
694
695	*(ogl_matrix + 1)  = mYAxis.mV[VX];
696	*(ogl_matrix + 5)  = mYAxis.mV[VY];
697	*(ogl_matrix + 9)  = mYAxis.mV[VZ];
698	*(ogl_matrix + 13) = -mOrigin * mYAxis;
699
700	*(ogl_matrix + 2)  = mZAxis.mV[VX];
701	*(ogl_matrix + 6)  = mZAxis.mV[VY];
702	*(ogl_matrix + 10) = mZAxis.mV[VZ];
703	*(ogl_matrix + 14) = -mOrigin * mZAxis;
704
705	*(ogl_matrix + 3)  = 0.0f;
706	*(ogl_matrix + 7)  = 0.0f;
707	*(ogl_matrix + 11) = 0.0f;
708	*(ogl_matrix + 15) = 1.0f;
709}
710
711
712// at and up_direction are presumed to be normalized
713void LLCoordFrame::lookDir(const LLVector3 &at, const LLVector3 &up_direction)
714{
715	// Make sure 'at' and 'up_direction' are not parallel
716	// and that neither are zero-length vectors
717	LLVector3 left(up_direction % at);
718	if (left.isNull()) 
719	{
720		//tweak lookat pos so we don't get a degenerate matrix
721		LLVector3 tempat(at[VX] + 0.01f, at[VY], at[VZ]);
722		tempat.normVec();
723		left = (up_direction % tempat);
724	}
725	left.normVec();
726
727	LLVector3 up = at % left;
728
729	if (at.isFinite() && left.isFinite() && up.isFinite())
730	{
731		setAxes(at, left, up);
732	}
733}
734
735void LLCoordFrame::lookDir(const LLVector3 &xuv)
736{
737	static LLVector3 up_direction(0.0f, 0.0f, 1.0f);
738	lookDir(xuv, up_direction);
739}
740
741void LLCoordFrame::lookAt(const LLVector3 &origin, const LLVector3 &point_of_interest, const LLVector3 &up_direction)
742{
743	setOrigin(origin);
744	LLVector3 at(point_of_interest - origin);
745	at.normVec();
746	lookDir(at, up_direction);
747}
748
749void LLCoordFrame::lookAt(const LLVector3 &origin, const LLVector3 &point_of_interest)
750{
751	static LLVector3 up_direction(0.0f, 0.0f, 1.0f);
752
753	setOrigin(origin);
754	LLVector3 at(point_of_interest - origin);
755	at.normVec();
756	lookDir(at, up_direction);
757}
758
759
760// Operators and friends
761
762std::ostream& operator<<(std::ostream &s, const LLCoordFrame &C)
763{
764	s << "{ "
765	  << " origin = " << C.mOrigin
766	  << " x_axis = " << C.mXAxis
767	  << " y_axis = " << C.mYAxis
768	  << " z_axis = " << C.mZAxis
769	<< " }";
770	return s;
771}
772
773
774
775// Private member functions
776
777
778//EOF