开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 游戏 > 游戏引擎 > 查看源码
mathlib.h
资源名称：Engine_v0.2.tar.gz　[点击查看]
上传用户：qccn516
上传日期：2013-05-02
资源大小：3382k
文件大小：30k
源码类别：

游戏引擎

开发平台：

Visual C++

mathlib.h：源码内容
  1. /* mathlib
  2.  *
  3.  * Copyright (C) 2003-2004, Alexander Zaprjagaev <frustum@frustum.org>
  4.  *
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or
  8.  * (at your option) any later version.
  9.  *
  10.  * This program 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
  13.  * GNU General Public License for more details.
  14.  *
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18.  */
  20. #ifndef __MATHLIB_H__
  21. #define __MATHLIB_H__
  23. #include <math.h>
  25. #define EPSILON 1e-6f
  26. #define PI 3.14159265358979323846f
  27. #define DEG2RAD (PI / 180.0f)
  28. #define RAD2DEG (180.0f / PI)
  30. struct vec2;
  31. struct vec3;
  32. struct vec4;
  33. struct mat3;
  34. struct mat4;
  35. struct quat;
  37. /*****************************************************************************/
  38. /*                                                                           */
  39. /* vec2                                                                      */
  40. /*                                                                           */
  41. /*****************************************************************************/
  43. struct vec2 {
  45. inline vec2() : x(0), y(0) { }
  46. inline vec2(float x,float y) : x(x), y(y) { }
  47. inline vec2(const float *v) : x(v[0]), y(v[1]) { }
  48. inline vec2(const vec2 &v) : x(v.x), y(v.y) { }
  50. inline int operator==(const vec2 &v) { return (fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON); }
  51. inline int operator!=(const vec2 &v) { return !(*this == v); }
  53. inline const vec2 operator*(float f) const { return vec2(x * f,y * f); }
  54. inline const vec2 operator/(float f) const { return vec2(x / f,y / f); }
  55. inline const vec2 operator+(const vec2 &v) const { return vec2(x + v.x,y + v.y); }
  56. inline const vec2 operator-() const { return vec2(-x,-y); }
  57. inline const vec2 operator-(const vec2 &v) const { return vec2(x - v.x,y - v.y); }
  59. inline vec2 &operator*=(float f) { return *this = *this * f; }
  60. inline vec2 &operator/=(float f) { return *this = *this / f; }
  61. inline vec2 &operator+=(const vec2 &v) { return *this = *this + v; }
  62. inline vec2 &operator-=(const vec2 &v) { return *this = *this - v; }
  64. inline float operator*(const vec2 &v) const { return x * v.x + y * v.y; }
  66. inline operator float*() { return (float*)&x; }
  67. inline operator const float*() const { return (float*)&x; }
  69. inline float &operator[](int i) { return ((float*)&x)[i]; }
  70. inline const float operator[](int i) const { return ((float*)&x)[i]; }
  72. inline float length() const { return sqrt(x * x + y * y); }
  73. inline float normalize() {
  74. float inv,length = sqrt(x * x + y * y);
  75. if(length < EPSILON) return 0.0;
  76. inv = 1.0f / length;
  77. x *= inv;
  78. y *= inv;
  79. return length;
  80. }
  82. union {
  83. struct {
  84. float x,y;
  85. };
  86. float v[2];
  87. };
  88. };
  90. /*****************************************************************************/
  91. /*                                                                           */
  92. /* vec3                                                                      */
  93. /*                                                                           */
  94. /*****************************************************************************/
  96. struct vec3 {
  98. inline vec3() : x(0), y(0), z(0) { }
  99. inline vec3(float x,float y,float z) : x(x), y(y), z(z) { }
  100. inline vec3(const float *v) : x(v[0]), y(v[1]), z(v[2]) { }
  101. inline vec3(const vec3 &v) : x(v.x), y(v.y), z(v.z) { }
  102. inline vec3(const vec4 &v);
  104. inline int operator==(const vec3 &v) { return (fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON && fabs(z - v.z) < EPSILON); }
  105. inline int operator!=(const vec3 &v) { return !(*this == v); }
  107. inline const vec3 operator*(float f) const { return vec3(x * f,y * f,z * f); }
  108. inline const vec3 operator/(float f) const { return vec3(x / f,y / f,z / f); }
  109. inline const vec3 operator+(const vec3 &v) const { return vec3(x + v.x,y + v.y,z + v.z); }
  110. inline const vec3 operator-() const { return vec3(-x,-y,-z); }
  111. inline const vec3 operator-(const vec3 &v) const { return vec3(x - v.x,y - v.y,z - v.z); }
  113. inline vec3 &operator*=(float f) { return *this = *this * f; }
  114. inline vec3 &operator/=(float f) { return *this = *this / f; }
  115. inline vec3 &operator+=(const vec3 &v) { return *this = *this + v; }
  116. inline vec3 &operator-=(const vec3 &v) { return *this = *this - v; }
  118. inline float operator*(const vec3 &v) const { return x * v.x + y * v.y + z * v.z; }
  119. inline float operator*(const vec4 &v) const;
  121. inline operator float*() { return (float*)&x; }
  122. inline operator const float*() const { return (float*)&x; }
  124. inline float &operator[](int i) { return ((float*)&x)[i]; }
  125. inline const float operator[](int i) const { return ((float*)&x)[i]; }
  127. inline float length() const { return sqrt(x * x + y * y + z * z); }
  128. inline float normalize() {
  129. float inv,length = sqrt(x * x + y * y + z * z);
  130. if(length < EPSILON) return 0.0;
  131. inv = 1.0f / length;
  132. x *= inv;
  133. y *= inv;
  134. z *= inv;
  135. return length;
  136. }
  137. inline void cross(const vec3 &v1,const vec3 &v2) {
  138. x = v1.y * v2.z - v1.z * v2.y;
  139. y = v1.z * v2.x - v1.x * v2.z;
  140. z = v1.x * v2.y - v1.y * v2.x;
  141. }
  143. union {
  144. struct {
  145. float x,y,z;
  146. };
  147. float v[3];
  148. };
  149. };
  151. inline vec3 cross(const vec3 &v1,const vec3 &v2) {
  152. vec3 ret;
  153. ret.x = v1.y * v2.z - v1.z * v2.y;
  154. ret.y = v1.z * v2.x - v1.x * v2.z;
  155. ret.z = v1.x * v2.y - v1.y * v2.x;
  156. return ret;
  157. }
  159. /*****************************************************************************/
  160. /*                                                                           */
  161. /* vec4                                                                      */
  162. /*                                                                           */
  163. /*****************************************************************************/
  165. struct vec4 {
  167. inline vec4() : x(0), y(0), z(0), w(1) { }
  168. inline vec4(float x,float y,float z,float w) : x(x), y(y), z(z), w(w) { }
  169. inline vec4(const float *v) : x(v[0]), y(v[1]), z(v[2]), w(v[3]) { }
  170. inline vec4(const vec3 &v) : x(v.x), y(v.y), z(v.z), w(1) { }
  171. inline vec4(const vec3 &v,float w) : x(v.x), y(v.y), z(v.z), w(w) { }
  172. inline vec4(const vec4 &v) : x(v.x), y(v.y), z(v.z), w(v.w) { }
  174. inline int operator==(const vec4 &v) { return (fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON && fabs(z - v.z) < EPSILON && fabs(w - v.w) < EPSILON); }
  175. inline int operator!=(const vec4 &v) { return !(*this == v); }
  177. inline const vec4 operator*(float f) const { return vec4(x * f,y * f,z * f,w * f); }
  178. inline const vec4 operator/(float f) const { return vec4(x / f,y / f,z / f,w / f); }
  179. inline const vec4 operator+(const vec4 &v) const { return vec4(x + v.x,y + v.y,z + v.z,w + v.w); }
  180. inline const vec4 operator-() const { return vec4(-x,-y,-z,-w); }
  181. inline const vec4 operator-(const vec4 &v) const { return vec4(x - v.x,y - v.y,z - v.z,z - v.w); }
  183. inline vec4 &operator*=(float f) { return *this = *this * f; }
  184. inline vec4 &operator/=(float f) { return *this = *this / f; }
  185. inline vec4 &operator+=(const vec4 &v) { return *this = *this + v; }
  186. inline vec4 &operator-=(const vec4 &v) { return *this = *this - v; }
  188. inline float operator*(const vec3 &v) const { return x * v.x + y * v.y + z * v.z + w; }
  189. inline float operator*(const vec4 &v) const { return x * v.x + y * v.y + z * v.z + w * v.w; }
  191. inline operator float*() { return (float*)&x; }
  192. inline operator const float*() const { return (float*)&x; }
  194. inline float &operator[](int i) { return ((float*)&x)[i]; }
  195. inline const float operator[](int i) const { return ((float*)&x)[i]; }
  197. union {
  198. struct {
  199. float x,y,z,w;
  200. };
  201. float v[4];
  202. };
  203. };
  205. inline vec3::vec3(const vec4 &v) {
  206. x = v.x;
  207. y = v.y;
  208. z = v.z;
  209. }
  211. inline float vec3::operator*(const vec4 &v) const {
  212. return x * v.x + y * v.y + z * v.z + v.w;
  213. }
  215. /*****************************************************************************/
  216. /*                                                                           */
  217. /* mat3                                                                      */
  218. /*                                                                           */
  219. /*****************************************************************************/
  221. struct mat3 {
  223. mat3() {
  224. mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
  225. mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
  226. mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
  227. }
  228. mat3(const float *m) {
  229. mat[0] = m[0]; mat[3] = m[3]; mat[6] = m[6];
  230. mat[1] = m[1]; mat[4] = m[4]; mat[7] = m[7];
  231. mat[2] = m[2]; mat[5] = m[5]; mat[8] = m[8];
  232. }
  233. mat3(const mat3 &m) {
  234. mat[0] = m[0]; mat[3] = m[3]; mat[6] = m[6];
  235. mat[1] = m[1]; mat[4] = m[4]; mat[7] = m[7];
  236. mat[2] = m[2]; mat[5] = m[5]; mat[8] = m[8];
  237. }
  238. mat3(const mat4 &m);
  240. vec3 operator*(const vec3 &v) const {
  241. vec3 ret;
  242. ret[0] = mat[0] * v[0] + mat[3] * v[1] + mat[6] * v[2];
  243. ret[1] = mat[1] * v[0] + mat[4] * v[1] + mat[7] * v[2];
  244. ret[2] = mat[2] * v[0] + mat[5] * v[1] + mat[8] * v[2];
  245. return ret;
  246. }
  247. vec4 operator*(const vec4 &v) const {
  248. vec4 ret;
  249. ret[0] = mat[0] * v[0] + mat[3] * v[1] + mat[6] * v[2];
  250. ret[1] = mat[1] * v[0] + mat[4] * v[1] + mat[7] * v[2];
  251. ret[2] = mat[2] * v[0] + mat[5] * v[1] + mat[8] * v[2];
  252. ret[3] = v[3];
  253. return ret;
  254. }
  255. mat3 operator*(float f) const {
  256. mat3 ret;
  257. ret[0] = mat[0] * f; ret[3] = mat[3] * f; ret[6] = mat[6] * f;
  258. ret[1] = mat[1] * f; ret[4] = mat[4] * f; ret[7] = mat[7] * f;
  259. ret[2] = mat[2] * f; ret[5] = mat[5] * f; ret[8] = mat[8] * f;
  260. return ret;
  261. }
  262. mat3 operator*(const mat3 &m) const {
  263. mat3 ret;
  264. ret[0] = mat[0] * m[0] + mat[3] * m[1] + mat[6] * m[2];
  265. ret[1] = mat[1] * m[0] + mat[4] * m[1] + mat[7] * m[2];
  266. ret[2] = mat[2] * m[0] + mat[5] * m[1] + mat[8] * m[2];
  267. ret[3] = mat[0] * m[3] + mat[3] * m[4] + mat[6] * m[5];
  268. ret[4] = mat[1] * m[3] + mat[4] * m[4] + mat[7] * m[5];
  269. ret[5] = mat[2] * m[3] + mat[5] * m[4] + mat[8] * m[5];
  270. ret[6] = mat[0] * m[6] + mat[3] * m[7] + mat[6] * m[8];
  271. ret[7] = mat[1] * m[6] + mat[4] * m[7] + mat[7] * m[8];
  272. ret[8] = mat[2] * m[6] + mat[5] * m[7] + mat[8] * m[8];
  273. return ret;
  274. }
  275. mat3 operator+(const mat3 &m) const {
  276. mat3 ret;
  277. ret[0] = mat[0] + m[0]; ret[3] = mat[3] + m[3]; ret[6] = mat[6] + m[6];
  278. ret[1] = mat[1] + m[1]; ret[4] = mat[4] + m[4]; ret[7] = mat[7] + m[7];
  279. ret[2] = mat[2] + m[2]; ret[5] = mat[5] + m[5]; ret[8] = mat[8] + m[8];
  280. return ret;
  281. }
  282. mat3 operator-(const mat3 &m) const {
  283. mat3 ret;
  284. ret[0] = mat[0] - m[0]; ret[3] = mat[3] - m[3]; ret[6] = mat[6] - m[6];
  285. ret[1] = mat[1] - m[1]; ret[4] = mat[4] - m[4]; ret[7] = mat[7] - m[7];
  286. ret[2] = mat[2] - m[2]; ret[5] = mat[5] - m[5]; ret[8] = mat[8] - m[8];
  287. return ret;
  288. }
  290. mat3 &operator*=(float f) { return *this = *this * f; }
  291. mat3 &operator*=(const mat3 &m) { return *this = *this * m; }
  292. mat3 &operator+=(const mat3 &m) { return *this = *this + m; }
  293. mat3 &operator-=(const mat3 &m) { return *this = *this - m; }
  295. operator float*() { return mat; }
  296. operator const float*() const { return mat; }
  298. float &operator[](int i) { return mat[i]; }
  299. const float operator[](int i) const { return mat[i]; }
  301. mat3 transpose() const {
  302. mat3 ret;
  303. ret[0] = mat[0]; ret[3] = mat[1]; ret[6] = mat[2];
  304. ret[1] = mat[3]; ret[4] = mat[4]; ret[7] = mat[5];
  305. ret[2] = mat[6]; ret[5] = mat[7]; ret[8] = mat[8];
  306. return ret;
  307. }
  308. float det() const {
  309. float det;
  310. det = mat[0] * mat[4] * mat[8];
  311. det += mat[3] * mat[7] * mat[2];
  312. det += mat[6] * mat[1] * mat[5];
  313. det -= mat[6] * mat[4] * mat[2];
  314. det -= mat[3] * mat[1] * mat[8];
  315. det -= mat[0] * mat[7] * mat[5];
  316. return det;
  317. }
  318. mat3 inverse() const {
  319. mat3 ret;
  320. float idet = 1.0f / det();
  321. ret[0] =  (mat[4] * mat[8] - mat[7] * mat[5]) * idet;
  322. ret[1] = -(mat[1] * mat[8] - mat[7] * mat[2]) * idet;
  323. ret[2] =  (mat[1] * mat[5] - mat[4] * mat[2]) * idet;
  324. ret[3] = -(mat[3] * mat[8] - mat[6] * mat[5]) * idet;
  325. ret[4] =  (mat[0] * mat[8] - mat[6] * mat[2]) * idet;
  326. ret[5] = -(mat[0] * mat[5] - mat[3] * mat[2]) * idet;
  327. ret[6] =  (mat[3] * mat[7] - mat[6] * mat[4]) * idet;
  328. ret[7] = -(mat[0] * mat[7] - mat[6] * mat[1]) * idet;
  329. ret[8] =  (mat[0] * mat[4] - mat[3] * mat[1]) * idet;
  330. return ret;
  331. }
  333. void zero() {
  334. mat[0] = 0.0; mat[3] = 0.0; mat[6] = 0.0;
  335. mat[1] = 0.0; mat[4] = 0.0; mat[7] = 0.0;
  336. mat[2] = 0.0; mat[5] = 0.0; mat[8] = 0.0;
  337. }
  338. void identity() {
  339. mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
  340. mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
  341. mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
  342. }
  343. void rotate(const vec3 &axis,float angle) {
  344. float rad = angle * DEG2RAD;
  345. float c = cos(rad);
  346. float s = sin(rad);
  347. vec3 v = axis;
  348. v.normalize();
  349. float xx = v.x * v.x;
  350. float yy = v.y * v.y;
  351. float zz = v.z * v.z;
  352. float xy = v.x * v.y;
  353. float yz = v.y * v.z;
  354. float zx = v.z * v.x;
  355. float xs = v.x * s;
  356. float ys = v.y * s;
  357. float zs = v.z * s;
  358. mat[0] = (1.0f - c) * xx + c; mat[3] = (1.0f - c) * xy - zs; mat[6] = (1.0f - c) * zx + ys;
  359. mat[1] = (1.0f - c) * xy + zs; mat[4] = (1.0f - c) * yy + c; mat[7] = (1.0f - c) * yz - xs;
  360. mat[2] = (1.0f - c) * zx - ys; mat[5] = (1.0f - c) * yz + xs; mat[8] = (1.0f - c) * zz + c;
  361. }
  362. void rotate(float x,float y,float z,float angle) {
  363. rotate(vec3(x,y,z),angle);
  364. }
  365. void rotate_x(float angle) {
  366. float rad = angle * DEG2RAD;
  367. float c = cos(rad);
  368. float s = sin(rad);
  369. mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
  370. mat[1] = 0.0; mat[4] = c; mat[7] = -s;
  371. mat[2] = 0.0; mat[5] = s; mat[8] = c;
  372. }
  373. void rotate_y(float angle) {
  374. float rad = angle * DEG2RAD;
  375. float c = cos(rad);
  376. float s = sin(rad);
  377. mat[0] = c; mat[3] = 0.0; mat[6] = s;
  378. mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
  379. mat[2] = -s; mat[5] = 0.0; mat[8] = c;
  380. }
  381. void rotate_z(float angle) {
  382. float rad = angle * DEG2RAD;
  383. float c = cos(rad);
  384. float s = sin(rad);
  385. mat[0] = c; mat[3] = -s; mat[6] = 0.0;
  386. mat[1] = s; mat[4] = c; mat[7] = 0.0;
  387. mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
  388. }
  389. void scale(const vec3 &v) {
  390. mat[0] = v.x; mat[3] = 0.0; mat[6] = 0.0;
  391. mat[1] = 0.0; mat[4] = v.y; mat[7] = 0.0;
  392. mat[2] = 0.0; mat[5] = 0.0; mat[8] = v.z;
  393. }
  394. void scale(float x,float y,float z) {
  395. scale(vec3(x,y,z));
  396. }
  397. void orthonormalize() {
  398. vec3 x(mat[0],mat[1],mat[2]);
  399. vec3 y(mat[3],mat[4],mat[5]);
  400. vec3 z;
  401. x.normalize();
  402. z.cross(x,y);
  403. z.normalize();
  404. y.cross(z,x);
  405. y.normalize();
  406. mat[0] = x.x; mat[3] = y.x; mat[6] = z.x;
  407. mat[1] = x.y; mat[4] = y.y; mat[7] = z.y;
  408. mat[2] = x.z; mat[5] = y.z; mat[8] = z.z;
  409. }
  411. float mat[9];
  412. };
  414. /*****************************************************************************/
  415. /*                                                                           */
  416. /* mat4                                                                      */
  417. /*                                                                           */
  418. /*****************************************************************************/
  420. struct mat4 {
  422. mat4() {
  423. mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
  424. mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
  425. mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
  426. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  427. }
  428. mat4(const vec3 &v) {
  429. translate(v);
  430. }
  431. mat4(float x,float y,float z) {
  432. translate(x,y,z);
  433. }
  434. mat4(const vec3 &axis,float angle) {
  435. rotate(axis,angle);
  436. }
  437. mat4(float x,float y,float z,float angle) {
  438. rotate(x,y,z,angle);
  439. }
  440. mat4(const mat3 &m) {
  441. mat[0] = m[0]; mat[4] = m[3]; mat[8] = m[6]; mat[12] = 0.0;
  442. mat[1] = m[1]; mat[5] = m[4]; mat[9] = m[7]; mat[13] = 0.0;
  443. mat[2] = m[2]; mat[6] = m[5]; mat[10] = m[8]; mat[14] = 0.0;
  444. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  445. }
  446. mat4(const float *m) {
  447. mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
  448. mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
  449. mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
  450. mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
  451. }
  452. mat4(const mat4 &m) {
  453. mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
  454. mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
  455. mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
  456. mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
  457. }
  459. vec3 operator*(const vec3 &v) const {
  460. vec3 ret;
  461. ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12];
  462. ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13];
  463. ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14];
  464. return ret;
  465. }
  466. vec4 operator*(const vec4 &v) const {
  467. vec4 ret;
  468. ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12] * v[3];
  469. ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13] * v[3];
  470. ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14] * v[3];
  471. ret[3] = mat[3] * v[0] + mat[7] * v[1] + mat[11] * v[2] + mat[15] * v[3];
  472. return ret;
  473. }
  474. mat4 operator*(float f) const {
  475. mat4 ret;
  476. ret[0] = mat[0] * f; ret[4] = mat[4] * f; ret[8] = mat[8] * f; ret[12] = mat[12] * f;
  477. ret[1] = mat[1] * f; ret[5] = mat[5] * f; ret[9] = mat[9] * f; ret[13] = mat[13] * f;
  478. ret[2] = mat[2] * f; ret[6] = mat[6] * f; ret[10] = mat[10] * f; ret[14] = mat[14] * f;
  479. ret[3] = mat[3] * f; ret[7] = mat[7] * f; ret[11] = mat[11] * f; ret[15] = mat[15] * f;
  480. return ret;
  481. }
  482. mat4 operator*(const mat4 &m) const {
  483. mat4 ret;
  484. ret[0] = mat[0] * m[0] + mat[4] * m[1] + mat[8] * m[2] + mat[12] * m[3];
  485. ret[1] = mat[1] * m[0] + mat[5] * m[1] + mat[9] * m[2] + mat[13] * m[3];
  486. ret[2] = mat[2] * m[0] + mat[6] * m[1] + mat[10] * m[2] + mat[14] * m[3];
  487. ret[3] = mat[3] * m[0] + mat[7] * m[1] + mat[11] * m[2] + mat[15] * m[3];
  488. ret[4] = mat[0] * m[4] + mat[4] * m[5] + mat[8] * m[6] + mat[12] * m[7];
  489. ret[5] = mat[1] * m[4] + mat[5] * m[5] + mat[9] * m[6] + mat[13] * m[7];
  490. ret[6] = mat[2] * m[4] + mat[6] * m[5] + mat[10] * m[6] + mat[14] * m[7];
  491. ret[7] = mat[3] * m[4] + mat[7] * m[5] + mat[11] * m[6] + mat[15] * m[7];
  492. ret[8] = mat[0] * m[8] + mat[4] * m[9] + mat[8] * m[10] + mat[12] * m[11];
  493. ret[9] = mat[1] * m[8] + mat[5] * m[9] + mat[9] * m[10] + mat[13] * m[11];
  494. ret[10] = mat[2] * m[8] + mat[6] * m[9] + mat[10] * m[10] + mat[14] * m[11];
  495. ret[11] = mat[3] * m[8] + mat[7] * m[9] + mat[11] * m[10] + mat[15] * m[11];
  496. ret[12] = mat[0] * m[12] + mat[4] * m[13] + mat[8] * m[14] + mat[12] * m[15];
  497. ret[13] = mat[1] * m[12] + mat[5] * m[13] + mat[9] * m[14] + mat[13] * m[15];
  498. ret[14] = mat[2] * m[12] + mat[6] * m[13] + mat[10] * m[14] + mat[14] * m[15];
  499. ret[15] = mat[3] * m[12] + mat[7] * m[13] + mat[11] * m[14] + mat[15] * m[15];
  500. return ret;
  501. }
  502. mat4 operator+(const mat4 &m) const {
  503. mat4 ret;
  504. ret[0] = mat[0] + m[0]; ret[4] = mat[4] + m[4]; ret[8] = mat[8] + m[8]; ret[12] = mat[12] + m[12];
  505. ret[1] = mat[1] + m[1]; ret[5] = mat[5] + m[5]; ret[9] = mat[9] + m[9]; ret[13] = mat[13] + m[13];
  506. ret[2] = mat[2] + m[2]; ret[6] = mat[6] + m[6]; ret[10] = mat[10] + m[10]; ret[14] = mat[14] + m[14];
  507. ret[3] = mat[3] + m[3]; ret[7] = mat[7] + m[7]; ret[11] = mat[11] + m[11]; ret[15] = mat[15] + m[15];
  508. return ret;
  509. }
  510. mat4 operator-(const mat4 &m) const {
  511. mat4 ret;
  512. ret[0] = mat[0] - m[0]; ret[4] = mat[4] - m[4]; ret[8] = mat[8] - m[8]; ret[12] = mat[12] - m[12];
  513. ret[1] = mat[1] - m[1]; ret[5] = mat[5] - m[5]; ret[9] = mat[9] - m[9]; ret[13] = mat[13] - m[13];
  514. ret[2] = mat[2] - m[2]; ret[6] = mat[6] - m[6]; ret[10] = mat[10] - m[10]; ret[14] = mat[14] - m[14];
  515. ret[3] = mat[3] - m[3]; ret[7] = mat[7] - m[7]; ret[11] = mat[11] - m[11]; ret[15] = mat[15] - m[15];
  516. return ret;
  517. }
  519. mat4 &operator*=(float f) { return *this = *this * f; }
  520. mat4 &operator*=(const mat4 &m) { return *this = *this * m; }
  521. mat4 &operator+=(const mat4 &m) { return *this = *this + m; }
  522. mat4 &operator-=(const mat4 &m) { return *this = *this - m; }
  524. operator float*() { return mat; }
  525. operator const float*() const { return mat; }
  527. float &operator[](int i) { return mat[i]; }
  528. const float operator[](int i) const { return mat[i]; }
  530. mat4 rotation() const {
  531. mat4 ret;
  532. ret[0] = mat[0]; ret[4] = mat[4]; ret[8] = mat[8]; ret[12] = 0;
  533. ret[1] = mat[1]; ret[5] = mat[5]; ret[9] = mat[9]; ret[13] = 0;
  534. ret[2] = mat[2]; ret[6] = mat[6]; ret[10] = mat[10]; ret[14] = 0;
  535. ret[3] = 0; ret[7] = 0; ret[11] = 0; ret[15] = 1;
  536. return ret;
  537. }
  538. mat4 transpose() const {
  539. mat4 ret;
  540. ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[3];
  541. ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[7];
  542. ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[11];
  543. ret[3] = mat[12]; ret[7] = mat[13]; ret[11] = mat[14]; ret[15] = mat[15];
  544. return ret;
  545. }
  546. mat4 transpose_rotation() const {
  547. mat4 ret;
  548. ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[12];
  549. ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[13];
  550. ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[14];
  551. ret[3] = mat[3]; ret[7] = mat[7]; ret[14] = mat[14]; ret[15] = mat[15];
  552. return ret;
  553. }
  555. float det() const {
  556. float det;
  557. det = mat[0] * mat[5] * mat[10];
  558. det += mat[4] * mat[9] * mat[2];
  559. det += mat[8] * mat[1] * mat[6];
  560. det -= mat[8] * mat[5] * mat[2];
  561. det -= mat[4] * mat[1] * mat[10];
  562. det -= mat[0] * mat[9] * mat[6];
  563. return det;
  564. }
  566. mat4 inverse() const {
  567. mat4 ret;
  568. float idet = 1.0f / det();
  569. ret[0] =  (mat[5] * mat[10] - mat[9] * mat[6]) * idet;
  570. ret[1] = -(mat[1] * mat[10] - mat[9] * mat[2]) * idet;
  571. ret[2] =  (mat[1] * mat[6] - mat[5] * mat[2]) * idet;
  572. ret[3] = 0.0;
  573. ret[4] = -(mat[4] * mat[10] - mat[8] * mat[6]) * idet;
  574. ret[5] =  (mat[0] * mat[10] - mat[8] * mat[2]) * idet;
  575. ret[6] = -(mat[0] * mat[6] - mat[4] * mat[2]) * idet;
  576. ret[7] = 0.0;
  577. ret[8] =  (mat[4] * mat[9] - mat[8] * mat[5]) * idet;
  578. ret[9] = -(mat[0] * mat[9] - mat[8] * mat[1]) * idet;
  579. ret[10] =  (mat[0] * mat[5] - mat[4] * mat[1]) * idet;
  580. ret[11] = 0.0;
  581. ret[12] = -(mat[12] * ret[0] + mat[13] * ret[4] + mat[14] * ret[8]);
  582. ret[13] = -(mat[12] * ret[1] + mat[13] * ret[5] + mat[14] * ret[9]);
  583. ret[14] = -(mat[12] * ret[2] + mat[13] * ret[6] + mat[14] * ret[10]);
  584. ret[15] = 1.0;
  585. return ret;
  586. }
  588. void zero() {
  589. mat[0] = 0.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
  590. mat[1] = 0.0; mat[5] = 0.0; mat[9] = 0.0; mat[13] = 0.0;
  591. mat[2] = 0.0; mat[6] = 0.0; mat[10] = 0.0; mat[14] = 0.0;
  592. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 0.0;
  593. }
  594. void identity() {
  595. mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
  596. mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
  597. mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
  598. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  599. }
  600. void rotate(const vec3 &axis,float angle) {
  601. float rad = angle * DEG2RAD;
  602. float c = cos(rad);
  603. float s = sin(rad);
  604. vec3 v = axis;
  605. v.normalize();
  606. float xx = v.x * v.x;
  607. float yy = v.y * v.y;
  608. float zz = v.z * v.z;
  609. float xy = v.x * v.y;
  610. float yz = v.y * v.z;
  611. float zx = v.z * v.x;
  612. float xs = v.x * s;
  613. float ys = v.y * s;
  614. float zs = v.z * s;
  615. mat[0] = (1.0f - c) * xx + c; mat[4] = (1.0f - c) * xy - zs; mat[8] = (1.0f - c) * zx + ys; mat[12] = 0.0;
  616. mat[1] = (1.0f - c) * xy + zs; mat[5] = (1.0f - c) * yy + c; mat[9] = (1.0f - c) * yz - xs; mat[13] = 0.0;
  617. mat[2] = (1.0f - c) * zx - ys; mat[6] = (1.0f - c) * yz + xs; mat[10] = (1.0f - c) * zz + c; mat[14] = 0.0;
  618. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  619. }
  620. void rotate(float x,float y,float z,float angle) {
  621. rotate(vec3(x,y,z),angle);
  622. }
  623. void rotate_x(float angle) {
  624. float rad = angle * DEG2RAD;
  625. float c = cos(rad);
  626. float s = sin(rad);
  627. mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
  628. mat[1] = 0.0; mat[5] = c; mat[9] = -s; mat[13] = 0.0;
  629. mat[2] = 0.0; mat[6] = s; mat[10] = c; mat[14] = 0.0;
  630. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  631. }
  632. void rotate_y(float angle) {
  633. float rad = angle * DEG2RAD;
  634. float c = cos(rad);
  635. float s = sin(rad);
  636. mat[0] = c; mat[4] = 0.0; mat[8] = s; mat[12] = 0.0;
  637. mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
  638. mat[2] = -s; mat[6] = 0.0; mat[10] = c; mat[14] = 0.0;
  639. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  640. }
  641. void rotate_z(float angle) {
  642. float rad = angle * DEG2RAD;
  643. float c = cos(rad);
  644. float s = sin(rad);
  645. mat[0] = c; mat[4] = -s; mat[8] = 0.0; mat[12] = 0.0;
  646. mat[1] = s; mat[5] = c; mat[9] = 0.0; mat[13] = 0.0;
  647. mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
  648. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  649. }
  650. void scale(const vec3 &v) {
  651. mat[0] = v.x; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
  652. mat[1] = 0.0; mat[5] = v.y; mat[9] = 0.0; mat[13] = 0.0;
  653. mat[2] = 0.0; mat[6] = 0.0; mat[10] = v.z; mat[14] = 0.0;
  654. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  655. }
  656. void scale(float x,float y,float z) {
  657. scale(vec3(x,y,z));
  658. }
  659. void translate(const vec3 &v) {
  660. mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = v.x;
  661. mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = v.y;
  662. mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = v.z;
  663. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  664. }
  665. void translate(float x,float y,float z) {
  666. translate(vec3(x,y,z));
  667. }
  668. void reflect(const vec4 &plane) {
  669. float x = plane.x;
  670. float y = plane.y;
  671. float z = plane.z;
  672. float x2 = x * 2.0f;
  673. float y2 = y * 2.0f;
  674. float z2 = z * 2.0f;
  675. mat[0] = 1.0f - x * x2; mat[4] = -y * x2; mat[8] = -z * x2; mat[12] = -plane.w * x2;
  676. mat[1] = -x * y2; mat[5] = 1.0f - y * y2; mat[9] = -z * y2; mat[13] = -plane.w * y2;
  677. mat[2] = -x * z2; mat[6] = -y * z2; mat[10] = 1.0f - z * z2; mat[14] = -plane.w * z2;
  678. mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
  679. }
  680. void reflect(float x,float y,float z,float w) {
  681. reflect(vec4(x,y,z,w));
  682. }
  684. void perspective(float fov,float aspect,float znear,float zfar) {
  685. float y = tan(fov * PI / 360.0f);
  686. float x = y * aspect;
  687. mat[0] = 1.0f / x; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
  688. mat[1] = 0.0; mat[5] = 1.0f / y; mat[9] = 0.0; mat[13] = 0.0;
  689. mat[2] = 0.0; mat[6] = 0.0; mat[10] = -(zfar + znear) / (zfar - znear); mat[14] = -(2.0f * zfar * znear) / (zfar - znear);
  690. mat[3] = 0.0; mat[7] = 0.0; mat[11] = -1.0; mat[15] = 0.0;
  691. }
  692. void look_at(const vec3 &eye,const vec3 &dir,const vec3 &up) {
  693. vec3 x,y,z;
  694. mat4 m0,m1;
  695. z = eye - dir;
  696. z.normalize();
  697. x.cross(up,z);
  698. x.normalize();
  699. y.cross(z,x);
  700. y.normalize();
  701. m0[0] = x.x; m0[4] = x.y; m0[8] = x.z; m0[12] = 0.0;
  702. m0[1] = y.x; m0[5] = y.y; m0[9] = y.z; m0[13] = 0.0;
  703. m0[2] = z.x; m0[6] = z.y; m0[10] = z.z; m0[14] = 0.0;
  704. m0[3] = 0.0; m0[7] = 0.0; m0[11] = 0.0; m0[15] = 1.0;
  705. m1.translate(-eye);
  706. *this = m0 * m1;
  707. }
  708. void look_at(const float *eye,const float *dir,const float *up) {
  709. look_at(vec3(eye),vec3(dir),vec3(up));
  710. }
  712. float mat[16];
  713. };
  715. inline mat3::mat3(const mat4 &m) {
  716. mat[0] = m[0]; mat[3] = m[4]; mat[6] = m[8];
  717. mat[1] = m[1]; mat[4] = m[5]; mat[7] = m[9];
  718. mat[2] = m[2]; mat[5] = m[6]; mat[8] = m[10];
  719. }
  721. /*****************************************************************************/
  722. /*                                                                           */
  723. /* quat                                                                      */
  724. /*                                                                           */
  725. /*****************************************************************************/
  727. struct quat {
  729. quat() : x(0), y(0), z(0), w(1) { }
  730. quat(const vec3 &dir,float angle) {
  731. set(dir,angle);
  732. }
  733. quat(float x,float y,float z,float angle) {
  734. set(x,y,z,angle);
  735. }
  736. quat(const mat3 &m) {
  737. float trace = m[0] + m[4] + m[8];
  738. if(trace > 0.0) {
  739. float s = sqrt(trace + 1.0f);
  740. q[3] = 0.5f * s;
  741. s = 0.5f / s;
  742. q[0] = (m[5] - m[7]) * s;
  743. q[1] = (m[6] - m[2]) * s;
  744. q[2] = (m[1] - m[3]) * s;
  745. } else {
  746. static int next[3] = { 1, 2, 0 };
  747. int i = 0;
  748. if(m[4] > m[0]) i = 1;
  749. if(m[8] > m[3 * i + i]) i = 2;
  750. int j = next[i];
  751. int k = next[j];
  752. float s = sqrt(m[3 * i + i] - m[3 * j + j] - m[3 * k + k] + 1.0f);
  753. q[i] = 0.5f * s;
  754. if(s != 0) s = 0.5f / s;
  755. q[3] = (m[3 * j + k] - m[3 * k + j]) * s;
  756. q[j] = (m[3 * i + j] + m[3 * j + i]) * s;
  757. q[k] = (m[3 * i + k] + m[3 * k + i]) * s;
  758. }
  759. }
  761. operator float*() { return (float*)&x; }
  762. operator const float*() const { return (float*)&x; }
  764. float &operator[](int i) { return ((float*)&x)[i]; }
  765. const float operator[](int i) const { return ((float*)&x)[i]; }
  767. quat operator*(const quat &q) const {
  768. quat ret;
  769. ret.x = w * q.x + x * q.x + y * q.z - z * q.y;
  770. ret.y = w * q.y + y * q.w + z * q.x - x * q.z;
  771. ret.z = w * q.z + z * q.w + x * q.y - y * q.x;
  772. ret.w = w * q.w - x * q.x - y * q.y - z * q.z;
  773. return ret;
  774. }
  776. void set(const vec3 &dir,float angle) {
  777. float length = dir.length();
  778. if(length != 0.0) {
  779. length = 1.0f / length;
  780. float sinangle = sin(angle * DEG2RAD / 2.0f);
  781. x = dir[0] * length * sinangle;
  782. y = dir[1] * length * sinangle;
  783. z = dir[2] * length * sinangle;
  784. w = cos(angle * DEG2RAD / 2.0f);
  785. } else {
  786. x = y = z = 0.0;
  787. w = 1.0;
  788. }
  789. }
  790. void set(float x,float y,float z,float angle) {
  791. set(vec3(x,y,z),angle);
  792. }
  794. void slerp(const quat &q0,const quat &q1,float t) {
  795. float k0,k1,cosomega = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
  796. quat q;
  797. if(cosomega < 0.0) {
  798. cosomega = -cosomega;
  799. q.x = -q1.x;
  800. q.y = -q1.y;
  801. q.z = -q1.z;
  802. q.w = -q1.w;
  803. } else {
  804. q.x = q1.x;
  805. q.y = q1.y;
  806. q.z = q1.z;
  807. q.w = q1.w;
  808. }
  809. if(1.0 - cosomega > 1e-6) {
  810. float omega = acos(cosomega);
  811. float sinomega = sin(omega);
  812. k0 = sin((1.0f - t) * omega) / sinomega;
  813. k1 = sin(t * omega) / sinomega;
  814. } else {
  815. k0 = 1.0f - t;
  816. k1 = t;
  817. }
  818. x = q0.x * k0 + q.x * k1;
  819. y = q0.y * k0 + q.y * k1;
  820. z = q0.z * k0 + q.z * k1;
  821. w = q0.w * k0 + q.w * k1;
  822. }
  824. mat3 to_matrix() const {
  825. mat3 ret;
  826. float x2 = x + x;
  827. float y2 = y + y;
  828. float z2 = z + z;
  829. float xx = x * x2;
  830. float yy = y * y2;
  831. float zz = z * z2;
  832. float xy = x * y2;
  833. float yz = y * z2;
  834. float xz = z * x2;
  835. float wx = w * x2;
  836. float wy = w * y2;
  837. float wz = w * z2;
  838. ret[0] = 1.0f - (yy + zz); ret[3] = xy - wz; ret[6] = xz + wy;
  839. ret[1] = xy + wz; ret[4] = 1.0f - (xx + zz); ret[7] = yz - wx;
  840. ret[2] = xz - wy; ret[5] = yz + wx; ret[8] = 1.0f - (xx + yy);
  841. return ret;
  842. }
  844. union {
  845. struct {
  846. float x,y,z,w;
  847. };
  848. float q[4];
  849. };
  850. };
  852. #endif /* __MATHLIB_H__ */