Revision: 9002
http://playerstage.svn.sourceforge.net/playerstage/?rev=9002&view=rev
Author: natepak
Date: 2010-12-04 02:43:10 +0000 (Sat, 04 Dec 2010)
Log Message:
-----------
Added matrix files
Added Paths:
-----------
code/gazebo/branches/dev/server/Matrix3.cc
code/gazebo/branches/dev/server/Matrix3.hh
code/gazebo/branches/dev/server/Matrix4.cc
code/gazebo/branches/dev/server/Matrix4.hh
code/gazebo/branches/dev/server/Messages.cc
code/gazebo/branches/dev/server/Plane.cc
code/gazebo/branches/dev/server/Plane.hh
Added: code/gazebo/branches/dev/server/Matrix3.cc
===================================================================
--- code/gazebo/branches/dev/server/Matrix3.cc (rev 0)
+++ code/gazebo/branches/dev/server/Matrix3.cc 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,67 @@
+#include <string.h>
+
+#include "GazeboError.hh"
+#include "Matrix3.hh"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Matrix3::Matrix3()
+{
+ memset(this->m, 0, sizeof(double)*9);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Matrix3::Matrix3(const Matrix3 &m)
+{
+ memcpy(this->m, m.m, sizeof(double)*9);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Matrix3::Matrix3(double v00, double v01, double v02,
+ double v10, double v11, double v12,
+ double v20, double v21, double v22)
+{
+ this->m[0][0] = v00;
+ this->m[0][1] = v01;
+ this->m[0][2] = v02;
+ this->m[1][0] = v10;
+ this->m[1][1] = v11;
+ this->m[1][2] = v12;
+ this->m[2][0] = v20;
+ this->m[2][1] = v21;
+ this->m[2][2] = v22;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Matrix3::~Matrix3()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set from axes
+void Matrix3::SetFromAxes(const Vector3 &xAxis, const Vector3 &yAxis,
+ const Vector3 &zAxis)
+{
+ this->SetCol(0, xAxis);
+ this->SetCol(1, yAxis);
+ this->SetCol(2, zAxis);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set a column
+void Matrix3::SetCol(unsigned int i, const Vector3 &v)
+{
+ if (i >= 3)
+ gzthrow("Invalid column number");
+
+ m[0][i] = v.x;
+ m[1][i] = v.y;
+ m[2][i] = v.z;
+}
Added: code/gazebo/branches/dev/server/Matrix3.hh
===================================================================
--- code/gazebo/branches/dev/server/Matrix3.hh (rev 0)
+++ code/gazebo/branches/dev/server/Matrix3.hh 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,28 @@
+#ifndef MATRIX3_HH
+#define MATRIX3_HH
+
+#include "Vector3.hh"
+
+namespace gazebo
+{
+ class Matrix3
+ {
+ public: Matrix3();
+ public: Matrix3(const Matrix3 &m);
+ public: Matrix3(double v00, double v01, double v02,
+ double v10, double v11, double v12,
+ double v20, double v21, double v22);
+
+ public: virtual ~Matrix3();
+
+ public: void SetFromAxes(const Vector3 &xAxis, const Vector3 &yAxis,
+ const Vector3 &zAxis);
+
+ /// Set a column
+ public: void SetCol(unsigned int i, const Vector3 &v);
+
+ protected: double m[3][3];
+ friend class Matrix4;
+ };
+}
+#endif
Added: code/gazebo/branches/dev/server/Matrix4.cc
===================================================================
--- code/gazebo/branches/dev/server/Matrix4.cc (rev 0)
+++ code/gazebo/branches/dev/server/Matrix4.cc 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,147 @@
+#include <string.h>
+
+#include "GazeboError.hh"
+#include "Matrix4.hh"
+
+using namespace gazebo;
+
+const Matrix4 Matrix4::IDENTITY(
+ 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0,
+ 0, 0, 0, 1 );
+
+const Matrix4 Matrix4::ZERO(
+ 0, 0, 0, 0,
+ 0, 0, 0, 0,
+ 0, 0, 0, 0,
+ 0, 0, 0, 0 );
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Default constructor
+Matrix4::Matrix4()
+{
+ memset(this->m, 0, sizeof(double)*16);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Copy constructor
+Matrix4::Matrix4(const Matrix4 &m)
+{
+ memcpy(this->m, m.m, sizeof(double)*16);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Matrix4::Matrix4(double v00, double v01, double v02, double v03,
+ double v10, double v11, double v12, double v13,
+ double v20, double v21, double v22, double v23,
+ double v30, double v31, double v32, double v33)
+{
+ this->m[0][0] = v00;
+ this->m[0][1] = v01;
+ this->m[0][2] = v02;
+ this->m[0][3] = v03;
+
+ this->m[1][0] = v10;
+ this->m[1][1] = v11;
+ this->m[1][2] = v12;
+ this->m[1][3] = v13;
+
+ this->m[2][0] = v20;
+ this->m[2][1] = v21;
+ this->m[2][2] = v22;
+ this->m[2][3] = v23;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Matrix4::~Matrix4()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set translation
+void Matrix4::SetTrans(const Vector3 &t)
+{
+ this->m[0][3] = t.x;
+ this->m[1][3] = t.y;
+ this->m[2][3] = t.z;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Equality operator
+const Matrix4 &Matrix4::operator=( const Matrix4 &mat )
+{
+ memcpy(this->m, mat.m, sizeof(double)*16);
+ return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Equality operator
+void Matrix4::operator=( const Matrix3 &mat )
+{
+ this->m[0][0] = mat.m[0][0];
+ this->m[0][1] = mat.m[0][1];
+ this->m[0][2] = mat.m[0][2];
+
+ this->m[1][0] = mat.m[1][0];
+ this->m[1][1] = mat.m[1][1];
+ this->m[1][2] = mat.m[1][2];
+
+ this->m[2][0] = mat.m[2][0];
+ this->m[2][1] = mat.m[2][1];
+ this->m[2][2] = mat.m[2][2];
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Mult operator
+Matrix4 Matrix4::operator*(const Matrix4 &m2)
+{
+ Matrix4 r;
+
+ r.m[0][0] = this->m[0][0] * m2.m[0][0] + this->m[0][1] * m2.m[1][0] +
this->m[0][2] * m2.m[2][0] + this->m[0][3] * m2.m[3][0];
+ r.m[0][1] = this->m[0][0] * m2.m[0][1] + this->m[0][1] * m2.m[1][1] +
this->m[0][2] * m2.m[2][1] + this->m[0][3] * m2.m[3][1];
+ r.m[0][2] = this->m[0][0] * m2.m[0][2] + this->m[0][1] * m2.m[1][2] +
this->m[0][2] * m2.m[2][2] + this->m[0][3] * m2.m[3][2];
+ r.m[0][3] = this->m[0][0] * m2.m[0][3] + this->m[0][1] * m2.m[1][3] +
this->m[0][2] * m2.m[2][3] + this->m[0][3] * m2.m[3][3];
+
+ r.m[1][0] = this->m[1][0] * m2.m[0][0] + this->m[1][1] * m2.m[1][0] +
this->m[1][2] * m2.m[2][0] + this->m[1][3] * m2.m[3][0];
+ r.m[1][1] = this->m[1][0] * m2.m[0][1] + this->m[1][1] * m2.m[1][1] +
this->m[1][2] * m2.m[2][1] + this->m[1][3] * m2.m[3][1];
+ r.m[1][2] = this->m[1][0] * m2.m[0][2] + this->m[1][1] * m2.m[1][2] +
this->m[1][2] * m2.m[2][2] + this->m[1][3] * m2.m[3][2];
+ r.m[1][3] = this->m[1][0] * m2.m[0][3] + this->m[1][1] * m2.m[1][3] +
this->m[1][2] * m2.m[2][3] + this->m[1][3] * m2.m[3][3];
+
+ r.m[2][0] = this->m[2][0] * m2.m[0][0] + this->m[2][1] * m2.m[1][0] +
this->m[2][2] * m2.m[2][0] + this->m[2][3] * m2.m[3][0];
+ r.m[2][1] = this->m[2][0] * m2.m[0][1] + this->m[2][1] * m2.m[1][1] +
this->m[2][2] * m2.m[2][1] + this->m[2][3] * m2.m[3][1];
+ r.m[2][2] = this->m[2][0] * m2.m[0][2] + this->m[2][1] * m2.m[1][2] +
this->m[2][2] * m2.m[2][2] + this->m[2][3] * m2.m[3][2];
+ r.m[2][3] = this->m[2][0] * m2.m[0][3] + this->m[2][1] * m2.m[1][3] +
this->m[2][2] * m2.m[2][3] + this->m[2][3] * m2.m[3][3];
+
+ r.m[3][0] = this->m[3][0] * m2.m[0][0] + this->m[3][1] * m2.m[1][0] +
this->m[3][2] * m2.m[2][0] + this->m[3][3] * m2.m[3][0];
+ r.m[3][1] = this->m[3][0] * m2.m[0][1] + this->m[3][1] * m2.m[1][1] +
this->m[3][2] * m2.m[2][1] + this->m[3][3] * m2.m[3][1];
+ r.m[3][2] = this->m[3][0] * m2.m[0][2] + this->m[3][1] * m2.m[1][2] +
this->m[3][2] * m2.m[2][2] + this->m[3][3] * m2.m[3][2];
+ r.m[3][3] = this->m[3][0] * m2.m[0][3] + this->m[3][1] * m2.m[1][3] +
this->m[3][2] * m2.m[2][3] + this->m[3][3] * m2.m[3][3];
+
+ return r;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Return true if affine
+bool Matrix4::IsAffine() const
+{
+ return this->m[3][0] == 0 && this->m[3][1] == 0 && this->m[3][2] == 0 &&
this->m[3][3] == 1;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Affine transform
+Vector3 Matrix4::TransformAffine( const Vector3 &v ) const
+{
+ if (!this->IsAffine())
+ gzthrow("Not and affine matrix");
+
+ return Vector3(
+ this->m[0][0]*v.x + this->m[0][1]*v.y + this->m[0][2]*v.z +
this->m[0][3],
+ this->m[1][0]*v.x + this->m[1][1]*v.y + this->m[1][2]*v.z +
this->m[1][3],
+ this->m[2][0]*v.x + this->m[2][1]*v.y + this->m[2][2]*v.z + this->m[2][3]
+ );
+}
Added: code/gazebo/branches/dev/server/Matrix4.hh
===================================================================
--- code/gazebo/branches/dev/server/Matrix4.hh (rev 0)
+++ code/gazebo/branches/dev/server/Matrix4.hh 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,51 @@
+#ifndef MATRIX4_HH
+#define MATRIX4_HH
+
+#include <iostream>
+
+#include "Vector3.hh"
+#include "Matrix3.hh"
+
+namespace gazebo
+{
+ class Matrix4
+ {
+ public: Matrix4();
+ public: Matrix4(const Matrix4 &m);
+ public: Matrix4(double v00, double v01, double v02, double v03,
+ double v10, double v11, double v12, double v13,
+ double v20, double v21, double v22, double v23,
+ double v30, double v31, double v32, double v33);
+
+ public: virtual ~Matrix4();
+
+ public: void SetTrans(const Vector3 &t);
+
+ public: bool IsAffine() const;
+ public: Vector3 TransformAffine( const Vector3 &v ) const;
+
+ public: const Matrix4 &operator=( const Matrix4 &mat );
+ public: void operator=( const Matrix3 &mat );
+
+ public: Matrix4 operator*(const Matrix4 &mat);
+
+ public: friend std::ostream &operator<<( std::ostream &out, const
gazebo::Matrix4 &m )
+ {
+ for (int i=0; i < 4; i++)
+ {
+ for (int j=0; j < 4; j++)
+ {
+ out << m.m[i][j] << " ";
+ }
+ out << "\n";
+ }
+ }
+
+ public: static const Matrix4 IDENTITY;
+ public: static const Matrix4 ZERO;
+
+
+ protected: double m[4][4];
+ };
+}
+#endif
Added: code/gazebo/branches/dev/server/Messages.cc
===================================================================
--- code/gazebo/branches/dev/server/Messages.cc (rev 0)
+++ code/gazebo/branches/dev/server/Messages.cc 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,29 @@
+#include "XMLConfig.hh"
+#include "Messages.hh"
+
+using namespace gazebo;
+
+VisualMsg::VisualMsg(const VisualMsg &m)
+ : Message(m)
+{
+ this->parentId = m.parentId;
+ this->id = m.id;
+ this->action = m.action;
+ this->render = m.render;
+ this->mesh = m.mesh;
+ this->material = m.material;
+ this->castShadows = m.castShadows;
+ this->attachAxes = m.attachAxes;
+ this->visible = m.visible;
+ this->transparency = m.transparency;
+ this->boundingbox = m.boundingbox;
+ this->points = m.points;
+ this->pose = m.pose;
+ this->plane = m.plane;
+ this->uvTile_x = m.uvTile_x;
+ this->uvTile_y = m.uvTile_y;
+}
+
+void VisualMsg::Load(XMLConfigNode *node)
+{
+}
Added: code/gazebo/branches/dev/server/Plane.cc
===================================================================
--- code/gazebo/branches/dev/server/Plane.cc (rev 0)
+++ code/gazebo/branches/dev/server/Plane.cc 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,30 @@
+#include "Plane.hh"
+
+using namespace gazebo;
+
+Plane::Plane()
+{
+ this->d = 0.0;
+}
+
+Plane::~Plane()
+{
+}
+
+void Plane::Set(Vector3 n, Vector2<double> s, double offset)
+{
+ this->normal = n;
+ this->size = s;
+ this->d = offset;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Equal operator
+const Plane &Plane::operator=(const Plane & p)
+{
+ this->normal = p.normal;
+ this->size = p.size;
+ this->d = p.d;
+
+ return *this;
+}
Added: code/gazebo/branches/dev/server/Plane.hh
===================================================================
--- code/gazebo/branches/dev/server/Plane.hh (rev 0)
+++ code/gazebo/branches/dev/server/Plane.hh 2010-12-04 02:43:10 UTC (rev
9002)
@@ -0,0 +1,25 @@
+#ifndef PLANE_HH
+#define PLANE_HH
+
+#include "Vector3.hh"
+#include "Vector2.hh"
+
+namespace gazebo
+{
+ class Plane
+ {
+ public: Plane();
+ public: virtual ~Plane();
+
+ public: void Set(Vector3 normal, Vector2<double> size, double offset);
+
+ /// \brief Equal operator
+ public: const Plane &operator=(const Plane & p);
+
+ public: Vector3 normal;
+ public: Vector2<double> size;
+ public: double d;
+ };
+}
+
+#endif
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
What happens now with your Lotus Notes apps - do you make another costly
upgrade, or settle for being marooned without product support? Time to move
off Lotus Notes and onto the cloud with Force.com, apps are easier to build,
use, and manage than apps on traditional platforms. Sign up for the Lotus
Notes Migration Kit to learn more. http://p.sf.net/sfu/salesforce-d2d
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
