Control program for a four-legged 12 axis robot.
Dependencies: CircularBuffer Servo Terminal mbed Radio
Diff: Matrix.cpp
- Revision:
- 5:475f67175510
- Parent:
- 3:6fa07ceb897f
- Child:
- 12:a952bd74d363
--- a/Matrix.cpp Wed Nov 14 04:29:55 2012 +0000 +++ b/Matrix.cpp Sun Jan 13 02:59:48 2013 +0000 @@ -4,6 +4,51 @@ +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 @@ -22,7 +67,7 @@ -matrix4& matrix4::translate(vector3 v) +matrix4& matrix4::translate(const vector3 v) { a14 += v.x; a24 += v.y;