Alex Chepilev / Mbed 2 deprecated WalkingRobot

1

Dependencies:   CircularBuffer Radio Servo Terminal mbed

Fork of WalkingRobot by Patrick Clary

Matrix.cpp@11:975d24305af7, 2013-04-09 (annotated)

Committer:
alex03
Date:
Tue Apr 09 01:40:27 2013 +0000
Revision:
11:975d24305af7
Parent:
5:475f67175510 
g

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pclary 3:6fa07ceb897f 1 #include "Matrix.h"
pclary 3:6fa07ceb897f 2 #include "mbed.h"
pclary 3:6fa07ceb897f 3 #include <cmath>
pclary 3:6fa07ceb897f 4
pclary 3:6fa07ceb897f 5
pclary 3:6fa07ceb897f 6
pclary 5:475f67175510 7 vector3 vector3::operator+(const vector3& v) const
pclary 5:475f67175510 8 {
pclary 5:475f67175510 9 vector3 r;
pclary 5:475f67175510 10 r.x = x + v.x;
pclary 5:475f67175510 11 r.y = y + v.y;
pclary 5:475f67175510 12 r.z = z + v.z;
pclary 5:475f67175510 13 return r;
pclary 5:475f67175510 14 }
pclary 5:475f67175510 15
pclary 5:475f67175510 16
pclary 5:475f67175510 17
pclary 5:475f67175510 18 vector3 vector3::operator-(const vector3& v) const
pclary 5:475f67175510 19 {
pclary 5:475f67175510 20 vector3 r;
pclary 5:475f67175510 21 r.x = x - v.x;
pclary 5:475f67175510 22 r.y = y - v.y;
pclary 5:475f67175510 23 r.z = z - v.z;
pclary 5:475f67175510 24 return r;
pclary 5:475f67175510 25 }
pclary 5:475f67175510 26
pclary 5:475f67175510 27
pclary 5:475f67175510 28
pclary 5:475f67175510 29 vector3 vector3::operator*(const float f) const
pclary 5:475f67175510 30 {
pclary 5:475f67175510 31 vector3 r;
pclary 5:475f67175510 32 r.x = x * f;
pclary 5:475f67175510 33 r.y = y * f;
pclary 5:475f67175510 34 r.z = z * f;
pclary 5:475f67175510 35 return r;
pclary 5:475f67175510 36 }
pclary 5:475f67175510 37
pclary 5:475f67175510 38
pclary 5:475f67175510 39
pclary 5:475f67175510 40 vector3 vector3::unit() const
pclary 5:475f67175510 41 {
pclary 5:475f67175510 42 vector3 r;
pclary 5:475f67175510 43 float m = sqrt(x*x + y*y + z*z);
pclary 5:475f67175510 44 r.x = x / m;
pclary 5:475f67175510 45 r.y = y / m;
pclary 5:475f67175510 46 r.z = z / m;
pclary 5:475f67175510 47 return r;
pclary 5:475f67175510 48 }
pclary 5:475f67175510 49
pclary 5:475f67175510 50
pclary 5:475f67175510 51
pclary 3:6fa07ceb897f 52 matrix4::matrix4()
pclary 3:6fa07ceb897f 53 {
pclary 3:6fa07ceb897f 54 // Initialize as identity matrix
pclary 3:6fa07ceb897f 55 identity();
pclary 3:6fa07ceb897f 56 }
pclary 3:6fa07ceb897f 57
pclary 3:6fa07ceb897f 58
pclary 3:6fa07ceb897f 59
pclary 3:6fa07ceb897f 60 matrix4& matrix4::identity()
pclary 3:6fa07ceb897f 61 {
pclary 3:6fa07ceb897f 62 a11 = 1.0f; a12 = 0.0f; a13 = 0.0f; a14 = 0.0f;
pclary 3:6fa07ceb897f 63 a21 = 0.0f; a22 = 1.0f; a23 = 0.0f; a24 = 0.0f;
pclary 3:6fa07ceb897f 64 a31 = 0.0f; a32 = 0.0f; a33 = 1.0f; a34 = 0.0f;
pclary 3:6fa07ceb897f 65 return *this;
pclary 3:6fa07ceb897f 66 }
pclary 3:6fa07ceb897f 67
pclary 3:6fa07ceb897f 68
pclary 3:6fa07ceb897f 69
pclary 5:475f67175510 70 matrix4& matrix4::translate(const vector3 v)
pclary 3:6fa07ceb897f 71 {
pclary 3:6fa07ceb897f 72 a14 += v.x;
pclary 3:6fa07ceb897f 73 a24 += v.y;
pclary 3:6fa07ceb897f 74 a34 += v.z;
pclary 3:6fa07ceb897f 75 return *this;
pclary 3:6fa07ceb897f 76 }
pclary 3:6fa07ceb897f 77
pclary 3:6fa07ceb897f 78
pclary 3:6fa07ceb897f 79
pclary 3:6fa07ceb897f 80 matrix4& matrix4::rotateX(float radians)
pclary 3:6fa07ceb897f 81 {
pclary 3:6fa07ceb897f 82 float b21 = a21;
pclary 3:6fa07ceb897f 83 float b22 = a22;
pclary 3:6fa07ceb897f 84 float b23 = a23;
pclary 3:6fa07ceb897f 85 float b24 = a24;
pclary 3:6fa07ceb897f 86 float sinx = sin(radians);
pclary 3:6fa07ceb897f 87 float cosx = cos(radians);
pclary 3:6fa07ceb897f 88
pclary 3:6fa07ceb897f 89 a21 = a21*cosx - a31*sinx;
pclary 3:6fa07ceb897f 90 a22 = a22*cosx - a32*sinx;
pclary 3:6fa07ceb897f 91 a23 = a23*cosx - a33*sinx;
pclary 3:6fa07ceb897f 92 a24 = a24*cosx - a34*sinx;
pclary 3:6fa07ceb897f 93
pclary 3:6fa07ceb897f 94 a31 = a31*cosx + b21*sinx;
pclary 3:6fa07ceb897f 95 a32 = a32*cosx + b22*sinx;
pclary 3:6fa07ceb897f 96 a33 = a33*cosx + b23*sinx;
pclary 3:6fa07ceb897f 97 a34 = a34*cosx + b24*sinx;
pclary 3:6fa07ceb897f 98
pclary 3:6fa07ceb897f 99 return *this;
pclary 3:6fa07ceb897f 100 }
pclary 3:6fa07ceb897f 101
pclary 3:6fa07ceb897f 102
pclary 3:6fa07ceb897f 103
pclary 3:6fa07ceb897f 104 matrix4& matrix4::rotateY(float radians)
pclary 3:6fa07ceb897f 105 {
pclary 3:6fa07ceb897f 106 float b31 = a31;
pclary 3:6fa07ceb897f 107 float b32 = a32;
pclary 3:6fa07ceb897f 108 float b33 = a33;
pclary 3:6fa07ceb897f 109 float b34 = a34;
pclary 3:6fa07ceb897f 110 float sinx = sin(radians);
pclary 3:6fa07ceb897f 111 float cosx = cos(radians);
pclary 3:6fa07ceb897f 112
pclary 3:6fa07ceb897f 113 a31 = a31*cosx - a11*sinx;
pclary 3:6fa07ceb897f 114 a32 = a32*cosx - a12*sinx;
pclary 3:6fa07ceb897f 115 a33 = a33*cosx - a13*sinx;
pclary 3:6fa07ceb897f 116 a34 = a34*cosx - a14*sinx;
pclary 3:6fa07ceb897f 117
pclary 3:6fa07ceb897f 118 a11 = a11*cosx + b31*sinx;
pclary 3:6fa07ceb897f 119 a12 = a12*cosx + b32*sinx;
pclary 3:6fa07ceb897f 120 a13 = a13*cosx + b33*sinx;
pclary 3:6fa07ceb897f 121 a14 = a14*cosx + b34*sinx;
pclary 3:6fa07ceb897f 122
pclary 3:6fa07ceb897f 123 return *this;
pclary 3:6fa07ceb897f 124 }
pclary 3:6fa07ceb897f 125
pclary 3:6fa07ceb897f 126
pclary 3:6fa07ceb897f 127
pclary 3:6fa07ceb897f 128 matrix4& matrix4::rotateZ(float radians)
pclary 3:6fa07ceb897f 129 {
pclary 3:6fa07ceb897f 130 float b11 = a11;
pclary 3:6fa07ceb897f 131 float b12 = a12;
pclary 3:6fa07ceb897f 132 float b13 = a13;
pclary 3:6fa07ceb897f 133 float b14 = a14;
pclary 3:6fa07ceb897f 134 float sinx = sin(radians);
pclary 3:6fa07ceb897f 135 float cosx = cos(radians);
pclary 3:6fa07ceb897f 136
pclary 3:6fa07ceb897f 137 a11 = a11*cosx - a21*sinx;
pclary 3:6fa07ceb897f 138 a12 = a12*cosx - a22*sinx;
pclary 3:6fa07ceb897f 139 a13 = a13*cosx - a23*sinx;
pclary 3:6fa07ceb897f 140 a14 = a14*cosx - a24*sinx;
pclary 3:6fa07ceb897f 141
pclary 3:6fa07ceb897f 142 a21 = a21*cosx + b11*sinx;
pclary 3:6fa07ceb897f 143 a22 = a22*cosx + b12*sinx;
pclary 3:6fa07ceb897f 144 a23 = a23*cosx + b13*sinx;
pclary 3:6fa07ceb897f 145 a24 = a24*cosx + b14*sinx;
pclary 3:6fa07ceb897f 146
pclary 3:6fa07ceb897f 147 return *this;
pclary 3:6fa07ceb897f 148 }
pclary 3:6fa07ceb897f 149
pclary 3:6fa07ceb897f 150
pclary 3:6fa07ceb897f 151
pclary 3:6fa07ceb897f 152 matrix4 matrix4::operator*(const matrix4& other) const
pclary 3:6fa07ceb897f 153 {
pclary 3:6fa07ceb897f 154 matrix4 result;
pclary 3:6fa07ceb897f 155
pclary 3:6fa07ceb897f 156 result.a11 = a11*other.a11 + a12*other.a21 + a13*other.a31;
pclary 3:6fa07ceb897f 157 result.a12 = a11*other.a12 + a12*other.a22 + a13*other.a32;
pclary 3:6fa07ceb897f 158 result.a13 = a11*other.a13 + a12*other.a23 + a13*other.a33;
pclary 3:6fa07ceb897f 159 result.a14 = a11*other.a14 + a12*other.a24 + a13*other.a34 + a14;
pclary 3:6fa07ceb897f 160
pclary 3:6fa07ceb897f 161 result.a21 = a21*other.a11 + a22*other.a21 + a23*other.a31;
pclary 3:6fa07ceb897f 162 result.a22 = a21*other.a12 + a22*other.a22 + a23*other.a32;
pclary 3:6fa07ceb897f 163 result.a23 = a21*other.a13 + a22*other.a23 + a23*other.a33;
pclary 3:6fa07ceb897f 164 result.a24 = a21*other.a14 + a22*other.a24 + a23*other.a34 + a24;
pclary 3:6fa07ceb897f 165
pclary 3:6fa07ceb897f 166 result.a31 = a31*other.a11 + a32*other.a21 + a33*other.a31;
pclary 3:6fa07ceb897f 167 result.a32 = a31*other.a12 + a32*other.a22 + a33*other.a32;
pclary 3:6fa07ceb897f 168 result.a33 = a31*other.a13 + a32*other.a23 + a33*other.a33;
pclary 3:6fa07ceb897f 169 result.a34 = a31*other.a14 + a32*other.a24 + a33*other.a34 + a34;
pclary 3:6fa07ceb897f 170
pclary 3:6fa07ceb897f 171 return result;
pclary 3:6fa07ceb897f 172 }
pclary 3:6fa07ceb897f 173
pclary 3:6fa07ceb897f 174
pclary 3:6fa07ceb897f 175
pclary 3:6fa07ceb897f 176 vector3 matrix4::operator*(const vector3& other) const
pclary 3:6fa07ceb897f 177 {
pclary 3:6fa07ceb897f 178 vector3 result;
pclary 3:6fa07ceb897f 179
pclary 3:6fa07ceb897f 180 result.x = a11*other.x + a12*other.y + a13*other.z + a14;
pclary 3:6fa07ceb897f 181 result.y = a21*other.x + a22*other.y + a23*other.z + a24;
pclary 3:6fa07ceb897f 182 result.z = a31*other.x + a32*other.y + a33*other.z + a34;
pclary 3:6fa07ceb897f 183
pclary 3:6fa07ceb897f 184 return result;
pclary 3:6fa07ceb897f 185 }
pclary 3:6fa07ceb897f 186
pclary 3:6fa07ceb897f 187
pclary 3:6fa07ceb897f 188
pclary 3:6fa07ceb897f 189 matrix4 matrix4::inverse() const
pclary 3:6fa07ceb897f 190 {
pclary 3:6fa07ceb897f 191 matrix4 result;
pclary 3:6fa07ceb897f 192 float idet = 1.0f/(a11*a22*a33 - a11*a23*a32 - a12*a21*a33 + a12*a23*a31 + a13*a21*a32 - a13*a22*a31);
pclary 3:6fa07ceb897f 193
pclary 3:6fa07ceb897f 194 result.a11 = (a22*a33 - a23*a32)*idet;
pclary 3:6fa07ceb897f 195 result.a12 = (a13*a32 - a12*a33)*idet;
pclary 3:6fa07ceb897f 196 result.a13 = (a12*a23 - a13*a22)*idet;
pclary 3:6fa07ceb897f 197 result.a14 = (a12*a24*a33 - a12*a23*a34 + a13*a22*a34 - a13*a24*a32 - a14*a22*a33 + a14*a23*a32)*idet;
pclary 3:6fa07ceb897f 198
pclary 3:6fa07ceb897f 199 result.a21 = (a23*a31 - a21*a33)*idet;
pclary 3:6fa07ceb897f 200 result.a22 = (a11*a33 - a13*a31)*idet;
pclary 3:6fa07ceb897f 201 result.a23 = (a13*a21 - a11*a23)*idet;
pclary 3:6fa07ceb897f 202 result.a24 = (a11*a23*a34 - a11*a24*a33 - a13*a21*a34 + a13*a24*a31 + a14*a21*a33 - a14*a23*a31)*idet;
pclary 3:6fa07ceb897f 203
pclary 3:6fa07ceb897f 204 result.a31 = (a21*a32 - a22*a31)*idet;
pclary 3:6fa07ceb897f 205 result.a32 = (a12*a31 - a11*a32)*idet;
pclary 3:6fa07ceb897f 206 result.a33 = (a11*a22 - a12*a21)*idet;
pclary 3:6fa07ceb897f 207 result.a34 = (a11*a24*a32 - a11*a22*a34 + a12*a21*a34 - a12*a24*a31 - a14*a21*a32 + a14*a22*a31)*idet;
pclary 3:6fa07ceb897f 208
pclary 3:6fa07ceb897f 209 return result;
pclary 3:6fa07ceb897f 210 }