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