Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h @ 2518

Last change on this file since 2518 was 2430, checked in by rgrieder, 16 years ago
Updated to Bullet 2.73 (first part).
Property svn:eol-style set to `native`
File size: 19.8 KB

Line
1	/*
2	Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3
4	This software is provided 'as-is', without any express or implied warranty.
5	In no event will the authors be held liable for any damages arising from the use of this software.
6	Permission is granted to anyone to use this software for any purpose,
7	including commercial applications, and to alter it and redistribute it freely,
8	subject to the following restrictions:
9
10	1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12	3. This notice may not be removed or altered from any source distribution.
13	*/
14
15
16	#ifndef btMatrix3x3_H
17	#define btMatrix3x3_H
18
19	#include "btScalar.h"
20
21	#include "btVector3.h"
22	#include "btQuaternion.h"
23
24
25
26	/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
27	* Make sure to only include a pure orthogonal matrix without scaling. */
28	class btMatrix3x3 {
29	public:
30	/** @brief No initializaion constructor */
31	btMatrix3x3 () {}
32
33	// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
34
35	/*@brief Constructor from Quaternion /
36	explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
37	/*
38	template <typename btScalar>
39	Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
40	{
41	setEulerYPR(yaw, pitch, roll);
42	}
43	*/
44	/** @brief Constructor with row major formatting */
45	btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
46	const btScalar& yx, const btScalar& yy, const btScalar& yz,
47	const btScalar& zx, const btScalar& zy, const btScalar& zz)
48	{
49	setValue(xx, xy, xz,
50	yx, yy, yz,
51	zx, zy, zz);
52	}
53	/** @brief Copy constructor */
54	SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
55	{
56	m_el[0] = other.m_el[0];
57	m_el[1] = other.m_el[1];
58	m_el[2] = other.m_el[2];
59	}
60	/** @brief Assignment Operator */
61	SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
62	{
63	m_el[0] = other.m_el[0];
64	m_el[1] = other.m_el[1];
65	m_el[2] = other.m_el[2];
66	return *this;
67	}
68
69	/** @brief Get a column of the matrix as a vector
70	* @param i Column number 0 indexed */
71	SIMD_FORCE_INLINE btVector3 getColumn(int i) const
72	{
73	return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
74	}
75
76
77	/** @brief Get a row of the matrix as a vector
78	* @param i Row number 0 indexed */
79	SIMD_FORCE_INLINE const btVector3& getRow(int i) const
80	{
81	btFullAssert(0 <= i && i < 3);
82	return m_el[i];
83	}
84
85	/** @brief Get a mutable reference to a row of the matrix as a vector
86	* @param i Row number 0 indexed */
87	SIMD_FORCE_INLINE btVector3& operator[](int i)
88	{
89	btFullAssert(0 <= i && i < 3);
90	return m_el[i];
91	}
92
93	/** @brief Get a const reference to a row of the matrix as a vector
94	* @param i Row number 0 indexed */
95	SIMD_FORCE_INLINE const btVector3& operator[](int i) const
96	{
97	btFullAssert(0 <= i && i < 3);
98	return m_el[i];
99	}
100
101	/** @brief Multiply by the target matrix on the right
102	* @param m Rotation matrix to be applied
103	* Equivilant to this = this * m */
104	btMatrix3x3& operator*=(const btMatrix3x3& m);
105
106	/** @brief Set from a carray of btScalars
107	* @param m A pointer to the beginning of an array of 9 btScalars */
108	void setFromOpenGLSubMatrix(const btScalar *m)
109	{
110	m_el[0].setValue(m[0],m[4],m[8]);
111	m_el[1].setValue(m[1],m[5],m[9]);
112	m_el[2].setValue(m[2],m[6],m[10]);
113
114	}
115	/** @brief Set the values of the matrix explicitly (row major)
116	* @param xx Top left
117	* @param xy Top Middle
118	* @param xz Top Right
119	* @param yx Middle Left
120	* @param yy Middle Middle
121	* @param yz Middle Right
122	* @param zx Bottom Left
123	* @param zy Bottom Middle
124	* @param zz Bottom Right*/
125	void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
126	const btScalar& yx, const btScalar& yy, const btScalar& yz,
127	const btScalar& zx, const btScalar& zy, const btScalar& zz)
128	{
129	m_el[0].setValue(xx,xy,xz);
130	m_el[1].setValue(yx,yy,yz);
131	m_el[2].setValue(zx,zy,zz);
132	}
133
134	/** @brief Set the matrix from a quaternion
135	* @param q The Quaternion to match */
136	void setRotation(const btQuaternion& q)
137	{
138	btScalar d = q.length2();
139	btFullAssert(d != btScalar(0.0));
140	btScalar s = btScalar(2.0) / d;
141	btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
142	btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
143	btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
144	btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
145	setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
146	xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
147	xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
148	}
149
150
151	/** @brief Set the matrix from euler angles using YPR around YXZ respectively
152	* @param yaw Yaw about Y axis
153	* @param pitch Pitch about X axis
154	* @param roll Roll about Z axis
155	*/
156	void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
157	{
158	setEulerZYX(roll, pitch, yaw);
159	}
160
161	/** @brief Set the matrix from euler angles YPR around ZYX axes
162	* @param eulerX Roll about X axis
163	* @param eulerY Pitch around Y axis
164	* @param eulerZ Yaw aboud Z axis
165	*
166	* These angles are used to produce a rotation matrix. The euler
167	* angles are applied in ZYX order. I.e a vector is first rotated
168	* about X then Y and then Z
169	**/
170	void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
171	///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
172	btScalar ci ( btCos(eulerX));
173	btScalar cj ( btCos(eulerY));
174	btScalar ch ( btCos(eulerZ));
175	btScalar si ( btSin(eulerX));
176	btScalar sj ( btSin(eulerY));
177	btScalar sh ( btSin(eulerZ));
178	btScalar cc = ci * ch;
179	btScalar cs = ci * sh;
180	btScalar sc = si * ch;
181	btScalar ss = si * sh;
182
183	setValue(cj * ch, sj * sc - cs, sj * cc + ss,
184	cj * sh, sj * ss + cc, sj * cs - sc,
185	-sj, cj * si, cj * ci);
186	}
187
188	/*@brief Set the matrix to the identity /
189	void setIdentity()
190	{
191	setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
192	btScalar(0.0), btScalar(1.0), btScalar(0.0),
193	btScalar(0.0), btScalar(0.0), btScalar(1.0));
194	}
195	/**@brief Fill the values of the matrix into a 9 element array
196	* @param m The array to be filled */
197	void getOpenGLSubMatrix(btScalar *m) const
198	{
199	m[0] = btScalar(m_el[0].x());
200	m[1] = btScalar(m_el[1].x());
201	m[2] = btScalar(m_el[2].x());
202	m[3] = btScalar(0.0);
203	m[4] = btScalar(m_el[0].y());
204	m[5] = btScalar(m_el[1].y());
205	m[6] = btScalar(m_el[2].y());
206	m[7] = btScalar(0.0);
207	m[8] = btScalar(m_el[0].z());
208	m[9] = btScalar(m_el[1].z());
209	m[10] = btScalar(m_el[2].z());
210	m[11] = btScalar(0.0);
211	}
212
213	/**@brief Get the matrix represented as a quaternion
214	* @param q The quaternion which will be set */
215	void getRotation(btQuaternion& q) const
216	{
217	btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
218	btScalar temp[4];
219
220	if (trace > btScalar(0.0))
221	{
222	btScalar s = btSqrt(trace + btScalar(1.0));
223	temp[3]=(s * btScalar(0.5));
224	s = btScalar(0.5) / s;
225
226	temp[0]=((m_el[2].y() - m_el[1].z()) * s);
227	temp[1]=((m_el[0].z() - m_el[2].x()) * s);
228	temp[2]=((m_el[1].x() - m_el[0].y()) * s);
229	}
230	else
231	{
232	int i = m_el[0].x() < m_el[1].y() ?
233	(m_el[1].y() < m_el[2].z() ? 2 : 1) :
234	(m_el[0].x() < m_el[2].z() ? 2 : 0);
235	int j = (i + 1) % 3;
236	int k = (i + 2) % 3;
237
238	btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
239	temp[i] = s * btScalar(0.5);
240	s = btScalar(0.5) / s;
241
242	temp[3] = (m_el[k][j] - m_el[j][k]) * s;
243	temp[j] = (m_el[j][i] + m_el[i][j]) * s;
244	temp[k] = (m_el[k][i] + m_el[i][k]) * s;
245	}
246	q.setValue(temp[0],temp[1],temp[2],temp[3]);
247	}
248
249	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
250	* @param yaw Yaw around Y axis
251	* @param pitch Pitch around X axis
252	* @param roll around Z axis */
253	void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
254	{
255
256	// first use the normal calculus
257	yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
258	pitch = btScalar(btAsin(-m_el[2].x()));
259	roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
260
261	// on pitch = +/-HalfPI
262	if (btFabs(pitch)==SIMD_HALF_PI)
263	{
264	if (yaw>0)
265	yaw-=SIMD_PI;
266	else
267	yaw+=SIMD_PI;
268
269	if (roll>0)
270	roll-=SIMD_PI;
271	else
272	roll+=SIMD_PI;
273	}
274	};
275
276
277	/**@brief Get the matrix represented as euler angles around ZYX
278	* @param yaw Yaw around X axis
279	* @param pitch Pitch around Y axis
280	* @param roll around X axis
281	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
282	void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
283	{
284	struct Euler{btScalar yaw, pitch, roll;};
285	Euler euler_out;
286	Euler euler_out2; //second solution
287	//get the pointer to the raw data
288
289	// Check that pitch is not at a singularity
290	if (btFabs(m_el[2].x()) >= 1)
291	{
292	euler_out.yaw = 0;
293	euler_out2.yaw = 0;
294
295	// From difference of angles formula
296	btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
297	if (m_el[2].x() > 0) //gimbal locked up
298	{
299	euler_out.pitch = SIMD_PI / btScalar(2.0);
300	euler_out2.pitch = SIMD_PI / btScalar(2.0);
301	euler_out.roll = euler_out.pitch + delta;
302	euler_out2.roll = euler_out.pitch + delta;
303	}
304	else // gimbal locked down
305	{
306	euler_out.pitch = -SIMD_PI / btScalar(2.0);
307	euler_out2.pitch = -SIMD_PI / btScalar(2.0);
308	euler_out.roll = -euler_out.pitch + delta;
309	euler_out2.roll = -euler_out.pitch + delta;
310	}
311	}
312	else
313	{
314	euler_out.pitch = - btAsin(m_el[2].x());
315	euler_out2.pitch = SIMD_PI - euler_out.pitch;
316
317	euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
318	m_el[2].z()/btCos(euler_out.pitch));
319	euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
320	m_el[2].z()/btCos(euler_out2.pitch));
321
322	euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
323	m_el[0].x()/btCos(euler_out.pitch));
324	euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
325	m_el[0].x()/btCos(euler_out2.pitch));
326	}
327
328	if (solution_number == 1)
329	{
330	yaw = euler_out.yaw;
331	pitch = euler_out.pitch;
332	roll = euler_out.roll;
333	}
334	else
335	{
336	yaw = euler_out2.yaw;
337	pitch = euler_out2.pitch;
338	roll = euler_out2.roll;
339	}
340	}
341
342	/**@brief Create a scaled copy of the matrix
343	* @param s Scaling vector The elements of the vector will scale each column */
344
345	btMatrix3x3 scaled(const btVector3& s) const
346	{
347	return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
348	m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
349	m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
350	}
351
352	/*@brief Return the determinant of the matrix /
353	btScalar determinant() const;
354	/*@brief Return the adjoint of the matrix /
355	btMatrix3x3 adjoint() const;
356	/*@brief Return the matrix with all values non negative /
357	btMatrix3x3 absolute() const;
358	/*@brief Return the transpose of the matrix /
359	btMatrix3x3 transpose() const;
360	/*@brief Return the inverse of the matrix /
361	btMatrix3x3 inverse() const;
362
363	btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
364	btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
365
366	SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
367	{
368	return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
369	}
370	SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
371	{
372	return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
373	}
374	SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
375	{
376	return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
377	}
378
379
380	/**@brief diagonalizes this matrix by the Jacobi method.
381	* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
382	* coordinate system, i.e., old_this = rot * new_this * rot^T.
383	* @param threshold See iteration
384	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
385	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
386	*
387	* Note that this matrix is assumed to be symmetric.
388	*/
389	void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
390	{
391	rot.setIdentity();
392	for (int step = maxSteps; step > 0; step--)
393	{
394	// find off-diagonal element [p][q] with largest magnitude
395	int p = 0;
396	int q = 1;
397	int r = 2;
398	btScalar max = btFabs(m_el[0][1]);
399	btScalar v = btFabs(m_el[0][2]);
400	if (v > max)
401	{
402	q = 2;
403	r = 1;
404	max = v;
405	}
406	v = btFabs(m_el[1][2]);
407	if (v > max)
408	{
409	p = 1;
410	q = 2;
411	r = 0;
412	max = v;
413	}
414
415	btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
416	if (max <= t)
417	{
418	if (max <= SIMD_EPSILON * t)
419	{
420	return;
421	}
422	step = 1;
423	}
424
425	// compute Jacobi rotation J which leads to a zero for element [p][q]
426	btScalar mpq = m_el[p][q];
427	btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
428	btScalar theta2 = theta * theta;
429	btScalar cos;
430	btScalar sin;
431	if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
432	{
433	t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
434	: 1 / (theta - btSqrt(1 + theta2));
435	cos = 1 / btSqrt(1 + t * t);
436	sin = cos * t;
437	}
438	else
439	{
440	// approximation for large theta-value, i.e., a nearly diagonal matrix
441	t = 1 / (theta * (2 + btScalar(0.5) / theta2));
442	cos = 1 - btScalar(0.5) * t * t;
443	sin = cos * t;
444	}
445
446	// apply rotation to matrix (this = J^T * this * J)
447	m_el[p][q] = m_el[q][p] = 0;
448	m_el[p][p] -= t * mpq;
449	m_el[q][q] += t * mpq;
450	btScalar mrp = m_el[r][p];
451	btScalar mrq = m_el[r][q];
452	m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
453	m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
454
455	// apply rotation to rot (rot = rot * J)
456	for (int i = 0; i < 3; i++)
457	{
458	btVector3& row = rot[i];
459	mrp = row[p];
460	mrq = row[q];
461	row[p] = cos * mrp - sin * mrq;
462	row[q] = cos * mrq + sin * mrp;
463	}
464	}
465	}
466
467
468
469	protected:
470	/**@brief Calculate the matrix cofactor
471	* @param r1 The first row to use for calculating the cofactor
472	* @param c1 The first column to use for calculating the cofactor
473	* @param r1 The second row to use for calculating the cofactor
474	* @param c1 The second column to use for calculating the cofactor
475	* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
476	*/
477	btScalar cofac(int r1, int c1, int r2, int c2) const
478	{
479	return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
480	}
481	///Data storage for the matrix, each vector is a row of the matrix
482	btVector3 m_el[3];
483	};
484
485	SIMD_FORCE_INLINE btMatrix3x3&
486	btMatrix3x3::operator*=(const btMatrix3x3& m)
487	{
488	setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
489	m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
490	m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
491	return *this;
492	}
493
494	SIMD_FORCE_INLINE btScalar
495	btMatrix3x3::determinant() const
496	{
497	return triple((this)[0], (this)[1], (*this)[2]);
498	}
499
500
501	SIMD_FORCE_INLINE btMatrix3x3
502	btMatrix3x3::absolute() const
503	{
504	return btMatrix3x3(
505	btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
506	btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
507	btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
508	}
509
510	SIMD_FORCE_INLINE btMatrix3x3
511	btMatrix3x3::transpose() const
512	{
513	return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
514	m_el[0].y(), m_el[1].y(), m_el[2].y(),
515	m_el[0].z(), m_el[1].z(), m_el[2].z());
516	}
517
518	SIMD_FORCE_INLINE btMatrix3x3
519	btMatrix3x3::adjoint() const
520	{
521	return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
522	cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
523	cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
524	}
525
526	SIMD_FORCE_INLINE btMatrix3x3
527	btMatrix3x3::inverse() const
528	{
529	btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
530	btScalar det = (*this)[0].dot(co);
531	btFullAssert(det != btScalar(0.0));
532	btScalar s = btScalar(1.0) / det;
533	return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
534	co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
535	co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
536	}
537
538	SIMD_FORCE_INLINE btMatrix3x3
539	btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
540	{
541	return btMatrix3x3(
542	m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
543	m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
544	m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
545	m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
546	m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
547	m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
548	m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
549	m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
550	m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
551	}
552
553	SIMD_FORCE_INLINE btMatrix3x3
554	btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
555	{
556	return btMatrix3x3(
557	m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
558	m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
559	m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
560
561	}
562
563	SIMD_FORCE_INLINE btVector3
564	operator*(const btMatrix3x3& m, const btVector3& v)
565	{
566	return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
567	}
568
569
570	SIMD_FORCE_INLINE btVector3
571	operator*(const btVector3& v, const btMatrix3x3& m)
572	{
573	return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
574	}
575
576	SIMD_FORCE_INLINE btMatrix3x3
577	operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
578	{
579	return btMatrix3x3(
580	m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
581	m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
582	m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
583	}
584
585	/*
586	SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
587	return btMatrix3x3(
588	m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
589	m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
590	m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
591	m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
592	m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
593	m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
594	m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
595	m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
596	m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
597	}
598	*/
599
600	/**@brief Equality operator between two matrices
601	* It will test all elements are equal. */
602	SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
603	{
604	return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
605	m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
606	m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
607	}
608
609	#endif

Note: See TracBrowser for help on using the repository browser.

Download in other formats: