1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
Matrix.cpp
- Committer:
- alex03
- Date:
- 2013-04-09
- Revision:
- 11:975d24305af7
- Parent:
- 5:475f67175510
File content as of revision 11:975d24305af7:
#include "Matrix.h" #include "mbed.h" #include <cmath> vector3 vector3::operator+(const vector3& v) const { vector3 r; r.x = x + v.x; r.y = y + v.y; r.z = z + v.z; return r; } vector3 vector3::operator-(const vector3& v) const { vector3 r; r.x = x - v.x; r.y = y - v.y; r.z = z - v.z; return r; } vector3 vector3::operator*(const float f) const { vector3 r; r.x = x * f; r.y = y * f; r.z = z * f; return r; } vector3 vector3::unit() const { vector3 r; float m = sqrt(x*x + y*y + z*z); r.x = x / m; r.y = y / m; r.z = z / m; return r; } matrix4::matrix4() { // Initialize as identity matrix identity(); } matrix4& matrix4::identity() { a11 = 1.0f; a12 = 0.0f; a13 = 0.0f; a14 = 0.0f; a21 = 0.0f; a22 = 1.0f; a23 = 0.0f; a24 = 0.0f; a31 = 0.0f; a32 = 0.0f; a33 = 1.0f; a34 = 0.0f; return *this; } matrix4& matrix4::translate(const vector3 v) { a14 += v.x; a24 += v.y; a34 += v.z; return *this; } matrix4& matrix4::rotateX(float radians) { float b21 = a21; float b22 = a22; float b23 = a23; float b24 = a24; float sinx = sin(radians); float cosx = cos(radians); a21 = a21*cosx - a31*sinx; a22 = a22*cosx - a32*sinx; a23 = a23*cosx - a33*sinx; a24 = a24*cosx - a34*sinx; a31 = a31*cosx + b21*sinx; a32 = a32*cosx + b22*sinx; a33 = a33*cosx + b23*sinx; a34 = a34*cosx + b24*sinx; return *this; } matrix4& matrix4::rotateY(float radians) { float b31 = a31; float b32 = a32; float b33 = a33; float b34 = a34; float sinx = sin(radians); float cosx = cos(radians); a31 = a31*cosx - a11*sinx; a32 = a32*cosx - a12*sinx; a33 = a33*cosx - a13*sinx; a34 = a34*cosx - a14*sinx; a11 = a11*cosx + b31*sinx; a12 = a12*cosx + b32*sinx; a13 = a13*cosx + b33*sinx; a14 = a14*cosx + b34*sinx; return *this; } matrix4& matrix4::rotateZ(float radians) { float b11 = a11; float b12 = a12; float b13 = a13; float b14 = a14; float sinx = sin(radians); float cosx = cos(radians); a11 = a11*cosx - a21*sinx; a12 = a12*cosx - a22*sinx; a13 = a13*cosx - a23*sinx; a14 = a14*cosx - a24*sinx; a21 = a21*cosx + b11*sinx; a22 = a22*cosx + b12*sinx; a23 = a23*cosx + b13*sinx; a24 = a24*cosx + b14*sinx; return *this; } matrix4 matrix4::operator*(const matrix4& other) const { matrix4 result; result.a11 = a11*other.a11 + a12*other.a21 + a13*other.a31; result.a12 = a11*other.a12 + a12*other.a22 + a13*other.a32; result.a13 = a11*other.a13 + a12*other.a23 + a13*other.a33; result.a14 = a11*other.a14 + a12*other.a24 + a13*other.a34 + a14; result.a21 = a21*other.a11 + a22*other.a21 + a23*other.a31; result.a22 = a21*other.a12 + a22*other.a22 + a23*other.a32; result.a23 = a21*other.a13 + a22*other.a23 + a23*other.a33; result.a24 = a21*other.a14 + a22*other.a24 + a23*other.a34 + a24; result.a31 = a31*other.a11 + a32*other.a21 + a33*other.a31; result.a32 = a31*other.a12 + a32*other.a22 + a33*other.a32; result.a33 = a31*other.a13 + a32*other.a23 + a33*other.a33; result.a34 = a31*other.a14 + a32*other.a24 + a33*other.a34 + a34; return result; } vector3 matrix4::operator*(const vector3& other) const { vector3 result; result.x = a11*other.x + a12*other.y + a13*other.z + a14; result.y = a21*other.x + a22*other.y + a23*other.z + a24; result.z = a31*other.x + a32*other.y + a33*other.z + a34; return result; } matrix4 matrix4::inverse() const { matrix4 result; float idet = 1.0f/(a11*a22*a33 - a11*a23*a32 - a12*a21*a33 + a12*a23*a31 + a13*a21*a32 - a13*a22*a31); result.a11 = (a22*a33 - a23*a32)*idet; result.a12 = (a13*a32 - a12*a33)*idet; result.a13 = (a12*a23 - a13*a22)*idet; result.a14 = (a12*a24*a33 - a12*a23*a34 + a13*a22*a34 - a13*a24*a32 - a14*a22*a33 + a14*a23*a32)*idet; result.a21 = (a23*a31 - a21*a33)*idet; result.a22 = (a11*a33 - a13*a31)*idet; result.a23 = (a13*a21 - a11*a23)*idet; result.a24 = (a11*a23*a34 - a11*a24*a33 - a13*a21*a34 + a13*a24*a31 + a14*a21*a33 - a14*a23*a31)*idet; result.a31 = (a21*a32 - a22*a31)*idet; result.a32 = (a12*a31 - a11*a32)*idet; result.a33 = (a11*a22 - a12*a21)*idet; result.a34 = (a11*a24*a32 - a11*a22*a34 + a12*a21*a34 - a12*a24*a31 - a14*a21*a32 + a14*a22*a31)*idet; return result; }