|
 |
d2b2b5 |
|
|
|
d2b2b5 |
#include "geometry.h"
|
|
|
d2b2b5 |
|
|
|
d2b2b5 |
|
|
|
e31ea0 |
Matrix4 Matrix4::operator*(const Matrix4 &m) const {
|
|
|
e31ea0 |
return Matrix4(
|
|
|
e31ea0 |
m00*m.m00 + m10*m.m01 + m20*m.m02 + m30*m.m02,
|
|
|
e31ea0 |
m01*m.m00 + m11*m.m01 + m21*m.m02 + m31*m.m02,
|
|
|
e31ea0 |
m02*m.m00 + m12*m.m01 + m22*m.m02 + m32*m.m02,
|
|
|
e31ea0 |
m03*m.m00 + m13*m.m01 + m23*m.m02 + m33*m.m02,
|
|
|
e31ea0 |
|
|
|
e31ea0 |
m00*m.m10 + m10*m.m11 + m20*m.m12 + m30*m.m12,
|
|
|
e31ea0 |
m01*m.m10 + m11*m.m11 + m21*m.m12 + m31*m.m12,
|
|
|
e31ea0 |
m02*m.m10 + m12*m.m11 + m22*m.m12 + m32*m.m12,
|
|
|
e31ea0 |
m03*m.m10 + m13*m.m11 + m23*m.m12 + m33*m.m12,
|
|
|
e31ea0 |
|
|
|
e31ea0 |
m00*m.m20 + m10*m.m21 + m20*m.m22 + m30*m.m22,
|
|
|
e31ea0 |
m01*m.m20 + m11*m.m21 + m21*m.m22 + m31*m.m22,
|
|
|
e31ea0 |
m02*m.m20 + m12*m.m21 + m22*m.m22 + m32*m.m22,
|
|
|
e31ea0 |
m03*m.m20 + m13*m.m21 + m23*m.m22 + m33*m.m22,
|
|
|
e31ea0 |
|
|
|
e31ea0 |
m00*m.m30 + m10*m.m31 + m20*m.m32 + m30*m.m32,
|
|
|
e31ea0 |
m01*m.m30 + m11*m.m31 + m21*m.m32 + m31*m.m32,
|
|
|
e31ea0 |
m02*m.m30 + m12*m.m31 + m22*m.m32 + m32*m.m32,
|
|
|
e31ea0 |
m03*m.m30 + m13*m.m31 + m23*m.m32 + m33*m.m32 );
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
Real Matrix4::det() const {
|
|
|
e31ea0 |
return m00 * (m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31))
|
|
|
e31ea0 |
+ m01 * (m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32))
|
|
|
e31ea0 |
+ m02 * (m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31))
|
|
|
e31ea0 |
+ m03 * (m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
Matrix4 Matrix4::inv() const {
|
|
|
e31ea0 |
Matrix4 r;
|
|
|
e31ea0 |
r.m00 = m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31);
|
|
|
e31ea0 |
r.m10 = m10*(m23*m32 - m22*m33) + m12*(m20*m33 - m23*m30) + m13*(m22*m30 - m20*m32);
|
|
|
e31ea0 |
r.m20 = m10*(m21*m33 - m23*m31) + m11*(m23*m30 - m20*m33) + m13*(m20*m31 - m21*m30);
|
|
|
e31ea0 |
r.m30 = m10*(m22*m31 - m21*m32) + m11*(m20*m32 - m22*m30) + m12*(m21*m30 - m20*m31);
|
|
|
e31ea0 |
|
|
|
e31ea0 |
double det = m00*r.m00 + m01*r.m10 + m02*r.m20 + m03*r.m30;
|
|
|
e31ea0 |
if (fabs(det) <= precision) return zero();
|
|
|
e31ea0 |
det = 1/det;
|
|
|
e31ea0 |
r.m00 *= det;
|
|
|
e31ea0 |
r.m10 *= det;
|
|
|
e31ea0 |
r.m20 *= det;
|
|
|
e31ea0 |
r.m30 *= det;
|
|
|
e31ea0 |
|
|
|
e31ea0 |
r.m01 = det*(m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32));
|
|
|
e31ea0 |
r.m11 = det*(m00*(m22*m33 - m23*m32) + m02*(m23*m30 - m20*m33) + m03*(m20*m32 - m22*m30));
|
|
|
e31ea0 |
r.m21 = det*(m00*(m23*m31 - m21*m33) + m01*(m20*m33 - m23*m30) + m03*(m21*m30 - m20*m31));
|
|
|
e31ea0 |
r.m31 = det*(m00*(m21*m32 - m22*m31) + m01*(m22*m30 - m20*m32) + m02*(m20*m31 - m21*m30));
|
|
|
e31ea0 |
r.m02 = det*(m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31));
|
|
|
e31ea0 |
r.m12 = det*(m00*(m13*m32 - m12*m33) + m02*(m10*m33 - m13*m30) + m03*(m12*m30 - m10*m32));
|
|
|
e31ea0 |
r.m22 = det*(m00*(m11*m33 - m13*m31) + m01*(m13*m30 - m10*m33) + m03*(m10*m31 - m11*m30));
|
|
|
e31ea0 |
r.m32 = det*(m00*(m12*m31 - m11*m32) + m01*(m10*m32 - m12*m30) + m02*(m11*m30 - m10*m31));
|
|
|
e31ea0 |
r.m03 = det*(m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));
|
|
|
e31ea0 |
r.m13 = det*(m00*(m12*m23 - m13*m22) + m02*(m13*m20 - m10*m23) + m03*(m10*m22 - m12*m20));
|
|
|
e31ea0 |
r.m23 = det*(m00*(m13*m21 - m11*m23) + m01*(m10*m23 - m13*m20) + m03*(m11*m20 - m10*m21));
|
|
|
e31ea0 |
r.m33 = det*(m00*(m11*m22 - m12*m21) + m01*(m12*m20 - m10*m22) + m02*(m10*m21 - m11*m20));
|
|
|
e31ea0 |
return r;
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
Matrix4 Matrix4::zero() {
|
|
|
e31ea0 |
return Matrix4(
|
|
|
e31ea0 |
0, 0, 0, 0,
|
|
|
e31ea0 |
0, 0, 0, 0,
|
|
|
e31ea0 |
0, 0, 0, 0,
|
|
|
e31ea0 |
0, 0, 0, 0 );
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
Matrix4 Matrix4::translation(const Vector3 &translate) {
|
|
|
e31ea0 |
return Matrix4(
|
|
|
e31ea0 |
1, 0, 0, 0,
|
|
|
e31ea0 |
0, 1, 0, 0,
|
|
|
e31ea0 |
0, 0, 1, 0,
|
|
|
e31ea0 |
translate.x, translate.y, translate.z, 1 );
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
Matrix4 Matrix4::scaling(const Vector3 &scale) {
|
|
|
e31ea0 |
return Matrix4(
|
|
|
e31ea0 |
scale.x, 0, 0, 0,
|
|
|
e31ea0 |
0, scale.y, 0, 0,
|
|
|
e31ea0 |
0, 0, scale.z, 0,
|
|
|
e31ea0 |
0, 0, 0, 1 );
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
Matrix4 Matrix4::rotation(const Vector3 &axis, Real angle) {
|
|
|
e31ea0 |
Real len = axis.len();
|
|
|
e31ea0 |
if (len < precision) return Matrix4();
|
|
|
e31ea0 |
Real s = sin(angle);
|
|
|
e31ea0 |
Real c = cos(angle);
|
|
|
e31ea0 |
Vector3 a = axis/len;
|
|
|
e31ea0 |
Vector3 ac = a*(1 - c);
|
|
|
e31ea0 |
return Matrix4(
|
|
|
e31ea0 |
a.x*ac.x + c , a.y*ac.x + a.z*s, a.z*ac.x - a.y*s, 0,
|
|
|
e31ea0 |
a.x*ac.y - a.z*s, a.y*ac.y + c , a.z*ac.y + a.x*s, 0,
|
|
|
e31ea0 |
a.x*ac.z + a.y*s, a.y*ac.z - a.x*s, a.z*ac.z + c , 0,
|
|
|
e31ea0 |
0 , 0 , 0 , 1 );
|
|
|
e31ea0 |
}
|
|
|
1bcd85 |
|
|
|
1bcd85 |
Matrix4 Matrix4::perspective(Real fovy, Real aspect, Real z_near, Real z_far) {
|
|
|
1bcd85 |
if (fabs(fovy) <= precision) return zero();
|
|
|
1bcd85 |
if (fabs(fovy) >= 180.0-precision) return zero();
|
|
|
1bcd85 |
if (fabs(aspect) <= precision) return zero();
|
|
|
1bcd85 |
if (fabs(z_near - z_far) <= precision) return zero();
|
|
|
1bcd85 |
|
|
|
1bcd85 |
Real f = 1/tan(0.5*fovy/180.0*M_PI);
|
|
|
1bcd85 |
Real d = 1/(z_near - z_far);
|
|
|
1bcd85 |
return Matrix4(
|
|
|
1bcd85 |
f/aspect, 0, 0 , 0,
|
|
|
1bcd85 |
0 , f, 0 , 0,
|
|
|
1bcd85 |
0 , 0, (z_near + z_far)*d, -1,
|
|
|
1bcd85 |
0 , 0, 2*z_near*z_far*d , 0 );
|
|
|
1bcd85 |
}
|
|
|
1bcd85 |
|