Patrick Clary / Mbed 2 deprecated WalkingRobot

Control program for a four-legged 12 axis robot.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

Matrix.cpp@22:9cf770fb12f8, 2014-09-30 (annotated)

Committer:
pclary
Date:
Tue Sep 30 07:31:07 2014 +0000
Revision:
22:9cf770fb12f8
Parent:
17:4ec59e8b52a6 
Hacked together a better demonstration

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 17:4ec59e8b52a6 40 vector3 vector3::operator/(const float f) const
pclary 5:475f67175510 41 {
pclary 5:475f67175510 42 vector3 r;
pclary 17:4ec59e8b52a6 43 r.x = x / f;
pclary 17:4ec59e8b52a6 44 r.y = y / f;
pclary 17:4ec59e8b52a6 45 r.z = z / f;
pclary 5:475f67175510 46 return r;
pclary 5:475f67175510 47 }
pclary 5:475f67175510 48
pclary 5:475f67175510 49
pclary 5:475f67175510 50
pclary 17:4ec59e8b52a6 51 float vector3::norm() const
pclary 17:4ec59e8b52a6 52 {
pclary 17:4ec59e8b52a6 53 return sqrt(x*x + y*y + z*z);
pclary 17:4ec59e8b52a6 54 }
pclary 17:4ec59e8b52a6 55
pclary 17:4ec59e8b52a6 56
pclary 17:4ec59e8b52a6 57
pclary 17:4ec59e8b52a6 58 vector3 vector3::unit() const
pclary 17:4ec59e8b52a6 59 {
pclary 17:4ec59e8b52a6 60 return (*this)/norm();
pclary 17:4ec59e8b52a6 61 }
pclary 17:4ec59e8b52a6 62
pclary 17:4ec59e8b52a6 63
pclary 17:4ec59e8b52a6 64
pclary 12:a952bd74d363 65 void vector3::print(char* buf, unsigned int len)
pclary 12:a952bd74d363 66 {
pclary 12:a952bd74d363 67 snprintf(buf, len, "%.4f\t%.4f\t%.4f", x, y, z);
pclary 12:a952bd74d363 68 }
pclary 12:a952bd74d363 69
pclary 12:a952bd74d363 70
pclary 12:a952bd74d363 71
pclary 3:6fa07ceb897f 72 matrix4::matrix4()
pclary 3:6fa07ceb897f 73 {
pclary 3:6fa07ceb897f 74 // Initialize as identity matrix
pclary 3:6fa07ceb897f 75 identity();
pclary 3:6fa07ceb897f 76 }
pclary 3:6fa07ceb897f 77
pclary 3:6fa07ceb897f 78
pclary 3:6fa07ceb897f 79
pclary 3:6fa07ceb897f 80 matrix4& matrix4::identity()
pclary 3:6fa07ceb897f 81 {
pclary 3:6fa07ceb897f 82 a11 = 1.0f; a12 = 0.0f; a13 = 0.0f; a14 = 0.0f;
pclary 3:6fa07ceb897f 83 a21 = 0.0f; a22 = 1.0f; a23 = 0.0f; a24 = 0.0f;
pclary 3:6fa07ceb897f 84 a31 = 0.0f; a32 = 0.0f; a33 = 1.0f; a34 = 0.0f;
pclary 3:6fa07ceb897f 85 return *this;
pclary 3:6fa07ceb897f 86 }
pclary 3:6fa07ceb897f 87
pclary 3:6fa07ceb897f 88
pclary 3:6fa07ceb897f 89
pclary 5:475f67175510 90 matrix4& matrix4::translate(const vector3 v)
pclary 3:6fa07ceb897f 91 {
pclary 3:6fa07ceb897f 92 a14 += v.x;
pclary 3:6fa07ceb897f 93 a24 += v.y;
pclary 3:6fa07ceb897f 94 a34 += v.z;
pclary 3:6fa07ceb897f 95 return *this;
pclary 3:6fa07ceb897f 96 }
pclary 3:6fa07ceb897f 97
pclary 3:6fa07ceb897f 98
pclary 3:6fa07ceb897f 99
pclary 3:6fa07ceb897f 100 matrix4& matrix4::rotateX(float radians)
pclary 3:6fa07ceb897f 101 {
pclary 3:6fa07ceb897f 102 float b21 = a21;
pclary 3:6fa07ceb897f 103 float b22 = a22;
pclary 3:6fa07ceb897f 104 float b23 = a23;
pclary 3:6fa07ceb897f 105 float b24 = a24;
pclary 3:6fa07ceb897f 106 float sinx = sin(radians);
pclary 3:6fa07ceb897f 107 float cosx = cos(radians);
pclary 3:6fa07ceb897f 108
pclary 3:6fa07ceb897f 109 a21 = a21*cosx - a31*sinx;
pclary 3:6fa07ceb897f 110 a22 = a22*cosx - a32*sinx;
pclary 3:6fa07ceb897f 111 a23 = a23*cosx - a33*sinx;
pclary 3:6fa07ceb897f 112 a24 = a24*cosx - a34*sinx;
pclary 3:6fa07ceb897f 113
pclary 3:6fa07ceb897f 114 a31 = a31*cosx + b21*sinx;
pclary 3:6fa07ceb897f 115 a32 = a32*cosx + b22*sinx;
pclary 3:6fa07ceb897f 116 a33 = a33*cosx + b23*sinx;
pclary 3:6fa07ceb897f 117 a34 = a34*cosx + b24*sinx;
pclary 3:6fa07ceb897f 118
pclary 3:6fa07ceb897f 119 return *this;
pclary 3:6fa07ceb897f 120 }
pclary 3:6fa07ceb897f 121
pclary 3:6fa07ceb897f 122
pclary 3:6fa07ceb897f 123
pclary 3:6fa07ceb897f 124 matrix4& matrix4::rotateY(float radians)
pclary 3:6fa07ceb897f 125 {
pclary 3:6fa07ceb897f 126 float b31 = a31;
pclary 3:6fa07ceb897f 127 float b32 = a32;
pclary 3:6fa07ceb897f 128 float b33 = a33;
pclary 3:6fa07ceb897f 129 float b34 = a34;
pclary 3:6fa07ceb897f 130 float sinx = sin(radians);
pclary 3:6fa07ceb897f 131 float cosx = cos(radians);
pclary 3:6fa07ceb897f 132
pclary 3:6fa07ceb897f 133 a31 = a31*cosx - a11*sinx;
pclary 3:6fa07ceb897f 134 a32 = a32*cosx - a12*sinx;
pclary 3:6fa07ceb897f 135 a33 = a33*cosx - a13*sinx;
pclary 3:6fa07ceb897f 136 a34 = a34*cosx - a14*sinx;
pclary 3:6fa07ceb897f 137
pclary 3:6fa07ceb897f 138 a11 = a11*cosx + b31*sinx;
pclary 3:6fa07ceb897f 139 a12 = a12*cosx + b32*sinx;
pclary 3:6fa07ceb897f 140 a13 = a13*cosx + b33*sinx;
pclary 3:6fa07ceb897f 141 a14 = a14*cosx + b34*sinx;
pclary 3:6fa07ceb897f 142
pclary 3:6fa07ceb897f 143 return *this;
pclary 3:6fa07ceb897f 144 }
pclary 3:6fa07ceb897f 145
pclary 3:6fa07ceb897f 146
pclary 3:6fa07ceb897f 147
pclary 3:6fa07ceb897f 148 matrix4& matrix4::rotateZ(float radians)
pclary 3:6fa07ceb897f 149 {
pclary 3:6fa07ceb897f 150 float b11 = a11;
pclary 3:6fa07ceb897f 151 float b12 = a12;
pclary 3:6fa07ceb897f 152 float b13 = a13;
pclary 3:6fa07ceb897f 153 float b14 = a14;
pclary 3:6fa07ceb897f 154 float sinx = sin(radians);
pclary 3:6fa07ceb897f 155 float cosx = cos(radians);
pclary 3:6fa07ceb897f 156
pclary 3:6fa07ceb897f 157 a11 = a11*cosx - a21*sinx;
pclary 3:6fa07ceb897f 158 a12 = a12*cosx - a22*sinx;
pclary 3:6fa07ceb897f 159 a13 = a13*cosx - a23*sinx;
pclary 3:6fa07ceb897f 160 a14 = a14*cosx - a24*sinx;
pclary 3:6fa07ceb897f 161
pclary 3:6fa07ceb897f 162 a21 = a21*cosx + b11*sinx;
pclary 3:6fa07ceb897f 163 a22 = a22*cosx + b12*sinx;
pclary 3:6fa07ceb897f 164 a23 = a23*cosx + b13*sinx;
pclary 3:6fa07ceb897f 165 a24 = a24*cosx + b14*sinx;
pclary 3:6fa07ceb897f 166
pclary 3:6fa07ceb897f 167 return *this;
pclary 3:6fa07ceb897f 168 }
pclary 3:6fa07ceb897f 169
pclary 3:6fa07ceb897f 170
pclary 3:6fa07ceb897f 171
pclary 3:6fa07ceb897f 172 matrix4 matrix4::operator*(const matrix4& other) const
pclary 3:6fa07ceb897f 173 {
pclary 3:6fa07ceb897f 174 matrix4 result;
pclary 3:6fa07ceb897f 175
pclary 3:6fa07ceb897f 176 result.a11 = a11*other.a11 + a12*other.a21 + a13*other.a31;
pclary 3:6fa07ceb897f 177 result.a12 = a11*other.a12 + a12*other.a22 + a13*other.a32;
pclary 3:6fa07ceb897f 178 result.a13 = a11*other.a13 + a12*other.a23 + a13*other.a33;
pclary 3:6fa07ceb897f 179 result.a14 = a11*other.a14 + a12*other.a24 + a13*other.a34 + a14;
pclary 3:6fa07ceb897f 180
pclary 3:6fa07ceb897f 181 result.a21 = a21*other.a11 + a22*other.a21 + a23*other.a31;
pclary 3:6fa07ceb897f 182 result.a22 = a21*other.a12 + a22*other.a22 + a23*other.a32;
pclary 3:6fa07ceb897f 183 result.a23 = a21*other.a13 + a22*other.a23 + a23*other.a33;
pclary 3:6fa07ceb897f 184 result.a24 = a21*other.a14 + a22*other.a24 + a23*other.a34 + a24;
pclary 3:6fa07ceb897f 185
pclary 3:6fa07ceb897f 186 result.a31 = a31*other.a11 + a32*other.a21 + a33*other.a31;
pclary 3:6fa07ceb897f 187 result.a32 = a31*other.a12 + a32*other.a22 + a33*other.a32;
pclary 3:6fa07ceb897f 188 result.a33 = a31*other.a13 + a32*other.a23 + a33*other.a33;
pclary 3:6fa07ceb897f 189 result.a34 = a31*other.a14 + a32*other.a24 + a33*other.a34 + a34;
pclary 3:6fa07ceb897f 190
pclary 3:6fa07ceb897f 191 return result;
pclary 3:6fa07ceb897f 192 }
pclary 3:6fa07ceb897f 193
pclary 3:6fa07ceb897f 194
pclary 3:6fa07ceb897f 195
pclary 3:6fa07ceb897f 196 vector3 matrix4::operator*(const vector3& other) const
pclary 3:6fa07ceb897f 197 {
pclary 3:6fa07ceb897f 198 vector3 result;
pclary 3:6fa07ceb897f 199
pclary 3:6fa07ceb897f 200 result.x = a11*other.x + a12*other.y + a13*other.z + a14;
pclary 3:6fa07ceb897f 201 result.y = a21*other.x + a22*other.y + a23*other.z + a24;
pclary 3:6fa07ceb897f 202 result.z = a31*other.x + a32*other.y + a33*other.z + a34;
pclary 3:6fa07ceb897f 203
pclary 3:6fa07ceb897f 204 return result;
pclary 3:6fa07ceb897f 205 }
pclary 3:6fa07ceb897f 206
pclary 3:6fa07ceb897f 207
pclary 3:6fa07ceb897f 208
pclary 3:6fa07ceb897f 209 matrix4 matrix4::inverse() const
pclary 3:6fa07ceb897f 210 {
pclary 3:6fa07ceb897f 211 matrix4 result;
pclary 3:6fa07ceb897f 212 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 213
pclary 3:6fa07ceb897f 214 result.a11 = (a22*a33 - a23*a32)*idet;
pclary 3:6fa07ceb897f 215 result.a12 = (a13*a32 - a12*a33)*idet;
pclary 3:6fa07ceb897f 216 result.a13 = (a12*a23 - a13*a22)*idet;
pclary 3:6fa07ceb897f 217 result.a14 = (a12*a24*a33 - a12*a23*a34 + a13*a22*a34 - a13*a24*a32 - a14*a22*a33 + a14*a23*a32)*idet;
pclary 3:6fa07ceb897f 218
pclary 3:6fa07ceb897f 219 result.a21 = (a23*a31 - a21*a33)*idet;
pclary 3:6fa07ceb897f 220 result.a22 = (a11*a33 - a13*a31)*idet;
pclary 3:6fa07ceb897f 221 result.a23 = (a13*a21 - a11*a23)*idet;
pclary 3:6fa07ceb897f 222 result.a24 = (a11*a23*a34 - a11*a24*a33 - a13*a21*a34 + a13*a24*a31 + a14*a21*a33 - a14*a23*a31)*idet;
pclary 3:6fa07ceb897f 223
pclary 3:6fa07ceb897f 224 result.a31 = (a21*a32 - a22*a31)*idet;
pclary 3:6fa07ceb897f 225 result.a32 = (a12*a31 - a11*a32)*idet;
pclary 3:6fa07ceb897f 226 result.a33 = (a11*a22 - a12*a21)*idet;
pclary 3:6fa07ceb897f 227 result.a34 = (a11*a24*a32 - a11*a22*a34 + a12*a21*a34 - a12*a24*a31 - a14*a21*a32 + a14*a22*a31)*idet;
pclary 3:6fa07ceb897f 228
pclary 3:6fa07ceb897f 229 return result;
pclary 12:a952bd74d363 230 }
pclary 12:a952bd74d363 231
pclary 12:a952bd74d363 232
pclary 12:a952bd74d363 233
pclary 12:a952bd74d363 234 void matrix4::print(char* buf, unsigned int len)
pclary 12:a952bd74d363 235 {
pclary 12:a952bd74d363 236 snprintf(buf, len, "%.4f\t%.4f\t%.4f\t%.4f\n"
pclary 12:a952bd74d363 237 "%.4f\t%.4f\t%.4f\t%.4f\n"
pclary 12:a952bd74d363 238 "%.4f\t%.4f\t%.4f\t%.4f\n"
pclary 12:a952bd74d363 239 "0 \t0 \t0 \t1\n",
pclary 12:a952bd74d363 240 a11, a12, a13, a14,
pclary 12:a952bd74d363 241 a21, a22, a23, a24,
pclary 12:a952bd74d363 242 a31, a32, a33, a34);
pclary 12:a952bd74d363 243 }