Vegastrike 0.5.1 rc1  1.0
Original sources for Vegastrike Evolved
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
opmatrix3.cpp
Go to the documentation of this file.
1 /*
2  Copyright (C) 1998,1999,2000 by Jorrit Tyberghein
3  Largely rewritten by Ivan Avramovic <ivan@avramovic.com>
4 
5  This library is free software; you can redistribute it and/or
6  modify it under the terms of the GNU Library General Public
7  License as published by the Free Software Foundation; either
8  version 2 of the License, or (at your option) any later version.
9 
10  This library is distributed in the hope that it will be useful,
11  but WITHOUT ANY WARRANTY; without even the implied warranty of
12  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13  Library General Public License for more details.
14 
15  You should have received a copy of the GNU Library General Public
16  License along with this library; if not, write to the Free
17  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18 */
19 
20 #include <math.h>
21 #include <float.h>
22 #include "cmd/collide2/opcodesysdef.h"
23 #include "cmd/collide2/opcodeqint.h"
24 #include "opmatrix3.h"
25 #include "gfx/quaternion.h"
26 
27 //---------------------------------------------------------------------------
28 
29 csMatrix3& csMatrix3::operator+= (const csMatrix3& m)
30 {
31  m11 += m.m11; m12 += m.m12; m13 += m.m13;
32  m21 += m.m21; m22 += m.m22; m23 += m.m23;
33  m31 += m.m31; m32 += m.m32; m33 += m.m33;
34  return *this;
35 }
36 
37 csMatrix3& csMatrix3::operator-= (const csMatrix3& m)
38 {
39  m11 -= m.m11; m12 -= m.m12; m13 -= m.m13;
40  m21 -= m.m21; m22 -= m.m22; m23 -= m.m23;
41  m31 -= m.m31; m32 -= m.m32; m33 -= m.m33;
42  return *this;
43 }
44 
45 csMatrix3& csMatrix3::operator*= (const csMatrix3& m)
46 {
47  float old_m11 = m11;
48  m11 = m11*m.m11 + m12*m.m21 + m13*m.m31;
49  float old_m12 = m12;
50  m12 = old_m11*m.m12 + m12*m.m22 + m13*m.m32;
51  m13 = old_m11*m.m13 + old_m12*m.m23 + m13*m.m33;
52  float old_m21 = m21;
53  m21 = m21*m.m11 + m22*m.m21 + m23*m.m31;
54  float old_m22 = m22;
55  m22 = old_m21*m.m12 + m22*m.m22 + m23*m.m32;
56  m23 = old_m21*m.m13 + old_m22*m.m23 + m23*m.m33;
57  float old_m31 = m31;
58  m31 = m31*m.m11 + m32*m.m21 + m33*m.m31;
59  float old_m32 = m32;
60  m32 = old_m31*m.m12 + m32*m.m22 + m33*m.m32;
61  m33 = old_m31*m.m13 + old_m32*m.m23 + m33*m.m33;
62  return *this;
63 }
64 
65 csMatrix3& csMatrix3::operator*= (float s)
66 {
67  m11 *= s; m12 *= s; m13 *= s;
68  m21 *= s; m22 *= s; m23 *= s;
69  m31 *= s; m32 *= s; m33 *= s;
70  return *this;
71 }
72 
73 void csMatrix3::Identity ()
74 {
75  m11 = m22 = m33 = 1.0;
76  m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
77 }
78 
79 bool csMatrix3::IsIdentity () const
80 {
81  return (m11 == 1.0) && (m22 == 1.0) && (m33 == 1.0)
82  && (m12 == 0.0) && (m13 == 0.0) && (m21 == 0.0)
83  && (m23 == 0.0) && (m31 == 0.0) && (m32 == 0.0);
84 }
85 
86 void csMatrix3::Transpose ()
87 {
88  float swap;
89  swap = m12; m12 = m21; m21 = swap;
90  swap = m13; m13 = m31; m31 = swap;
91  swap = m23; m23 = m32; m32 = swap;
92 }
93 
94 csMatrix3 csMatrix3::GetTranspose () const
95 {
96  csMatrix3 t;
97  t.m12 = m21; t.m21 = m12;
98  t.m13 = m31; t.m31 = m13;
99  t.m23 = m32; t.m32 = m23;
100  t.m11 = m11; t.m22 = m22; t.m33 = m33;
101  return t;
102 }
103 
104 float csMatrix3::Determinant () const
105 {
106  return
107  m11 * (m22*m33 - m23*m32)
108  -m12 * (m21*m33 - m23*m31)
109  +m13 * (m21*m32 - m22*m31);
110 }
111 
112 void csMatrix3::Set (const Quaternion &quat)
113 {
114  float s;
115  float xs, ys, zs;
116  float wx, wy, wz;
117  float xx, xy, xz;
118  float yy, yz, zz;
119  float x = quat.v.i;
120  float y = quat.v.j;
121  float z = quat.v.k;
122  float w = quat.s;
123 
124  // For unit Quat, just set s = 2 or set xs = x + x, etc.
125  s = 2 / (x * x + y * y + z * z + w * w );
126 
127  xs = x * s; ys = y * s; zs = z * s;
128  wx = w * xs; wy = w * ys; wz = w * zs;
129  xx = x * xs; xy = x * ys; xz = x * zs;
130  yy = y * ys; yz = y * zs; zz = z * zs;
131 
132  m11 = 1.0f - (yy + zz);
133  m12 = xy + wz;
134  m13 = xz - wy;
135  m21 = xy - wz;
136  m22 = 1.0f - (xx + zz);
137  m23 = yz + wx;
138  m31 = xz + wy;
139  m32 = yz - wx;
140  m33 = 1.0f - (xx + yy);
141 }
142 
143 
144 csMatrix3 operator+ (const csMatrix3& m1, const csMatrix3& m2)
145 {
146  return csMatrix3 (m1.m11+m2.m11, m1.m12+m2.m12, m1.m13+m2.m13,
147  m1.m21+m2.m21, m1.m22+m2.m22, m1.m23+m2.m23,
148  m1.m31+m2.m31, m1.m32+m2.m32, m1.m33+m2.m33);
149 }
150 
151 csMatrix3 operator- (const csMatrix3& m1, const csMatrix3& m2)
152 {
153  return csMatrix3 (m1.m11-m2.m11, m1.m12-m2.m12, m1.m13-m2.m13,
154  m1.m21-m2.m21, m1.m22-m2.m22, m1.m23-m2.m23,
155  m1.m31-m2.m31, m1.m32-m2.m32, m1.m33-m2.m33);
156 }
157 csMatrix3 operator* (const csMatrix3& m1, const csMatrix3& m2)
158 {
159  return csMatrix3 (
160  m1.m11*m2.m11 + m1.m12*m2.m21 + m1.m13*m2.m31,
161  m1.m11*m2.m12 + m1.m12*m2.m22 + m1.m13*m2.m32,
162  m1.m11*m2.m13 + m1.m12*m2.m23 + m1.m13*m2.m33,
163  m1.m21*m2.m11 + m1.m22*m2.m21 + m1.m23*m2.m31,
164  m1.m21*m2.m12 + m1.m22*m2.m22 + m1.m23*m2.m32,
165  m1.m21*m2.m13 + m1.m22*m2.m23 + m1.m23*m2.m33,
166  m1.m31*m2.m11 + m1.m32*m2.m21 + m1.m33*m2.m31,
167  m1.m31*m2.m12 + m1.m32*m2.m22 + m1.m33*m2.m32,
168  m1.m31*m2.m13 + m1.m32*m2.m23 + m1.m33*m2.m33 );
169 }
170 
171 csMatrix3 operator* (const csMatrix3& m, float f)
172 {
173  return csMatrix3 (m.m11*f, m.m12*f, m.m13*f,
174  m.m21*f, m.m22*f, m.m23*f,
175  m.m31*f, m.m32*f, m.m33*f);
176 }
177 
178 csMatrix3 operator* (float f, const csMatrix3& m)
179 {
180  return csMatrix3 (m.m11*f, m.m12*f, m.m13*f,
181  m.m21*f, m.m22*f, m.m23*f,
182  m.m31*f, m.m32*f, m.m33*f);
183 }
184 
185 csMatrix3 operator/ (const csMatrix3& m, float f)
186 {
187  float inv_f = 1 / f;
188  return csMatrix3 (m.m11*inv_f, m.m12*inv_f, m.m13*inv_f,
189  m.m21*inv_f, m.m22*inv_f, m.m23*inv_f,
190  m.m31*inv_f, m.m32*inv_f, m.m33*inv_f);
191 }
192 
193 bool operator== (const csMatrix3& m1, const csMatrix3& m2)
194 {
195  if (m1.m11 != m2.m11 || m1.m12 != m2.m12 || m1.m13 != m2.m13) return false;
196  if (m1.m21 != m2.m21 || m1.m22 != m2.m22 || m1.m23 != m2.m23) return false;
197  if (m1.m31 != m2.m31 || m1.m32 != m2.m32 || m1.m33 != m2.m33) return false;
198  return true;
199 }
200 
201 bool operator!= (const csMatrix3& m1, const csMatrix3& m2)
202 {
203  if (m1.m11 != m2.m11 || m1.m12 != m2.m12 || m1.m13 != m2.m13) return true;
204  if (m1.m21 != m2.m21 || m1.m22 != m2.m22 || m1.m23 != m2.m23) return true;
205  if (m1.m31 != m2.m31 || m1.m32 != m2.m32 || m1.m33 != m2.m33) return true;
206  return false;
207 }
208 
209 bool operator< (const csMatrix3& m, float f)
210 {
211  return ABS(m.m11)<f && ABS(m.m12)<f && ABS(m.m13)<f &&
212  ABS(m.m21)<f && ABS(m.m22)<f && ABS(m.m23)<f &&
213  ABS(m.m31)<f && ABS(m.m32)<f && ABS(m.m33)<f;
214 }
215 
216 bool operator> (float f, const csMatrix3& m)
217 {
218  return ABS(m.m11)<f && ABS(m.m12)<f && ABS(m.m13)<f &&
219  ABS(m.m21)<f && ABS(m.m22)<f && ABS(m.m23)<f &&
220  ABS(m.m31)<f && ABS(m.m32)<f && ABS(m.m33)<f;
221 }
222 
223 //---------------------------------------------------------------------------
224 
225 csXRotMatrix3::csXRotMatrix3 (float angle)
226 {
227  m11 = 1; m12 = 0; m13 = 0;
228  m21 = 0; m22 = cos (angle); m23 = -sin(angle);
229  m31 = 0; m32 = sin (angle); m33 = cos(angle);
230 }
231 
232 csYRotMatrix3::csYRotMatrix3 (float angle)
233 {
234  m11 = cos (angle); m12 = 0; m13 = -sin(angle);
235  m21 = 0; m22 = 1; m23 = 0;
236  m31 = sin (angle); m32 = 0; m33 = cos(angle);
237 }
238 
239 csZRotMatrix3::csZRotMatrix3 (float angle)
240 {
241  m11 = cos (angle); m12 = -sin(angle); m13 = 0;
242  m21 = sin (angle); m22 = cos(angle); m23 = 0;
243  m31 = 0; m32 = 0; m33 = 1;
244 }
245 
246 //---------------------------------------------------------------------------