simspark-cvs

[Markus R. <rol...@us...>]

Update of /cvsroot/simspark/simspark/spark/oxygen/physicsserver
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv14965/spark/oxygen/physicsserver

Modified Files:
	body.cpp body.h collider.cpp collider.h hingejoint.cpp 
	hingejoint.h universaljoint.cpp universaljoint.h 
Log Message:
- merged ROSIMPORTER_XLAB bracnh

Index: hingejoint.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/hingejoint.cpp,v
retrieving revision 1.5
retrieving revision 1.6
diff -C2 -d -r1.5 -r1.6
*** hingejoint.cpp	19 Feb 2006 13:15:26 -0000	1.5
--- hingejoint.cpp	15 Jun 2007 08:48:17 -0000	1.6
***************
*** 118,127 ****
  }
  
! float HingeJoint::GetAngle()
  {
      return gRadToDeg(dJointGetHingeAngle(mODEJoint));
  }
  
! float HingeJoint::GetAngleRate()
  {
      return gRadToDeg(dJointGetHingeAngleRate(mODEJoint));
--- 118,127 ----
  }
  
! float HingeJoint::GetAngle() const
  {
      return gRadToDeg(dJointGetHingeAngle(mODEJoint));
  }
  
! float HingeJoint::GetAngleRate() const
  {
      return gRadToDeg(dJointGetHingeAngleRate(mODEJoint));

Index: body.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/body.cpp,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** body.cpp	31 Mar 2007 13:19:56 -0000	1.4
--- body.cpp	15 Jun 2007 08:48:17 -0000	1.5
***************
*** 159,235 ****
  }
  
  void Body::SetSphere(float density, float radius)
  {
      dMass ODEMass;
!     dMassSetSphere(&ODEMass, density, radius);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
  void Body::SetSphereTotal(float total_mass, float radius)
  {
      dMass ODEMass;
!     dMassSetSphereTotal(&ODEMass, total_mass, radius);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
  void Body::SetBox(float density, const Vector3f& size)
  {
      dMass ODEMass;
!     dMassSetBox(&ODEMass, density, size[0], size[1], size[2]);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::SetBoxTotal(float total_mass, const salt::Vector3f& size)
  {
      dMass ODEMass;
!     dMassSetBoxTotal(&ODEMass, total_mass, size[0], size[1], size[2]);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::SetCylinder (float density, float radius, float length)
  {
      dMass ODEMass;
  
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCylinder (&ODEMass, density, direction, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::SetCylinderTotal(float total_mass, float radius, float length)
  {
      dMass ODEMass;
  
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCylinderTotal(&ODEMass, total_mass, direction, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::SetCappedCylinder (float density, float radius, float length)
  {
      dMass ODEMass;
  
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCappedCylinder (&ODEMass, density, direction, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::SetCappedCylinderTotal(float total_mass, float radius, float length)
  {
      dMass ODEMass;
  
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCappedCylinderTotal(&ODEMass, total_mass,
!                                 direction, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
  Vector3f Body::GetVelocity() const
  {
--- 159,346 ----
  }
  
+ void Body::PrepareSphere(dMass& mass, float density, float radius) const
+ {
+     dMassSetSphere(&mass, density, radius);
+ }
+ 
  void Body::SetSphere(float density, float radius)
  {
      dMass ODEMass;
!     PrepareSphere(ODEMass, density, radius);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
+ void Body::Addphere(float density, float radius, const Matrix& matrix)
+ {
+     dMass ODEMass;
+     PrepareSphere(ODEMass, density, radius);
+     AddMass(ODEMass, matrix);
+ }
+ 
+ void Body::PrepareSphereTotal(dMass& mass, float total_mass, float radius) const
+ {
+     dMassSetSphereTotal(&mass, total_mass, radius);
+ }
+ 
  void Body::SetSphereTotal(float total_mass, float radius)
  {
      dMass ODEMass;
!     PrepareSphereTotal(ODEMass, total_mass, radius);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
+ void Body::AddSphereTotal(float total_mass, float radius, const Matrix& matrix)
+ {
+     dMass ODEMass;
+     PrepareSphereTotal(ODEMass, total_mass, radius);
+     AddMass(ODEMass, matrix);
+ }
+ 
+ void Body::PrepareBox(dMass& mass, float density, const Vector3f& size) const
+ {
+     dMassSetBox(&mass, density, size[0], size[1], size[2]);
+ }
+ 
  void Body::SetBox(float density, const Vector3f& size)
  {
      dMass ODEMass;
!     PrepareBox(ODEMass, density, size);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::AddBox(float density, const Vector3f& size, const Matrix& matrix)
  {
      dMass ODEMass;
!     PrepareBox(ODEMass, density, size);
!     AddMass(ODEMass, matrix);
! }
! 
! void Body::PrepareBoxTotal(dMass& mass, float total_mass, const Vector3f& size) const
! {
!     dMassSetBoxTotal(&mass, total_mass, size[0], size[1], size[2]);
! }
! 
! void Body::SetBoxTotal(float total_mass, const Vector3f& size)
! {
!     dMass ODEMass;
!     PrepareBoxTotal(ODEMass, total_mass, size);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::AddBoxTotal(float total_mass, const Vector3f& size, const Matrix& matrix)
  {
      dMass ODEMass;
+     PrepareBoxTotal(ODEMass, total_mass, size);
+     AddMass(ODEMass, matrix);
+ }
  
+ void Body::AddMass(const dMass& mass, const Matrix& matrix)
+ {
+     dMass transMass(mass);
+ 
+     dMatrix3 rot;
+     ConvertRotationMatrix(matrix, rot);
+     dMassRotate(&transMass,rot);
+ 
+     dMass bodyMass;
+     dBodyGetMass(mODEBody, &bodyMass);
+     dMassAdd(&bodyMass, &transMass);
+     dBodySetMass(mODEBody, (const dMass*)&bodyMass);
+ 
+     /** ODE currently requires that the center mass is always in the
+         origin of the body
+     */
+     const Vector3f& trans(matrix.Pos());
+     dMassTranslate(&transMass,trans[0],trans[1],trans[2]);
+ }
+ 
+ void Body::PrepareCylinder (dMass& mass, float density, float radius, float length) const
+ {
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCylinder (&mass, density, direction, radius, length);
! }
! 
! void Body::SetCylinder (float density, float radius, float length)
! {
!     dMass ODEMass;
!     PrepareCylinder(ODEMass, density, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::AddCylinder (float density, float radius, float length, const Matrix& matrix)
  {
      dMass ODEMass;
+     PrepareCylinder(ODEMass, density, radius, length);
+     AddMass(ODEMass, matrix);
+ }
  
+ void Body::PrepareCylinderTotal(dMass& mass, float total_mass, float radius, float length) const
+ {
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCylinderTotal(&mass, total_mass, direction, radius, length);
! }
! 
! void Body::SetCylinderTotal(float total_mass, float radius, float length)
! {
!     dMass ODEMass;
!     PrepareCylinderTotal(ODEMass, total_mass, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::AddCylinderTotal(float total_mass, float radius, float length, const Matrix& matrix)
  {
      dMass ODEMass;
+     PrepareCylinderTotal(ODEMass, total_mass, radius, length);
+     AddMass(ODEMass, matrix);
+ }
  
+ void Body::PrepareCappedCylinder (dMass& mass, float density, float radius, float length) const
+ {
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCappedCylinder (&mass, density, direction, radius, length);
! }
! 
! void Body::SetCappedCylinder (float density, float radius, float length)
! {
!     dMass ODEMass;
!     PrepareCappedCylinder(ODEMass, density, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
! void Body::AddCappedCylinder (float density, float radius, float length, const Matrix& matrix)
  {
      dMass ODEMass;
+     PrepareCappedCylinder(ODEMass, density, radius, length);
+     AddMass(ODEMass, matrix);
+ }
  
+ void Body::PrepareCappedCylinderTotal(dMass& mass, float total_mass, float radius, float length) const
+ {
      // direction: (1=x, 2=y, 3=z)
      int direction = 3;
  
!     dMassSetCappedCylinderTotal(&mass, total_mass, direction, radius, length);
! }
! 
! void Body::SetCappedCylinderTotal(float total_mass, float radius, float length)
! {
!     dMass ODEMass;
!     PrepareCappedCylinderTotal(ODEMass, total_mass, radius, length);
      dBodySetMass(mODEBody, &ODEMass);
  }
  
+ void Body::AddCappedCylinderTotal(float total_mass, float radius, float length, const salt::Matrix& matrix)
+ {
+     dMass ODEMass;
+     PrepareCappedCylinderTotal(ODEMass, total_mass, radius, length);
+     AddMass(ODEMass, matrix);
+ }
+ 
  Vector3f Body::GetVelocity() const
  {
***************
*** 355,359 ****
  }
  
! void Body::TranslateMass(const salt::Vector3f& v)
  {
      dMass m;
--- 466,470 ----
  }
  
! void Body::TranslateMass(const Vector3f& v)
  {
      dMass m;

Index: universaljoint.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/universaljoint.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** universaljoint.h	12 Feb 2006 11:36:52 -0000	1.3
--- universaljoint.h	15 Jun 2007 08:48:17 -0000	1.4
***************
*** 48,57 ****
          \param axis a vector describing the axis in relative coordinates
      */
!     void SetAxis1(salt::Vector3f & axis);
  
      /** This function sets up the second axis of the joint
          \param axis a vector describing the axis in local coordinates
      */
!     void SetAxis2(salt::Vector3f & axis);
  
      /** returns the vector describing one of the two axis
--- 48,57 ----
          \param axis a vector describing the axis in relative coordinates
      */
!     void SetAxis1(const salt::Vector3f & axis);
  
      /** This function sets up the second axis of the joint
          \param axis a vector describing the axis in local coordinates
      */
!     void SetAxis2(const salt::Vector3f & axis);
  
      /** returns the vector describing one of the two axis
***************
*** 59,63 ****
          \param idx index of the desired axis
      */
!     salt::Vector3f GetAxis(EAxisIndex idx);
  
      /** returns one of the axis angles in degrees, measured between
--- 59,63 ----
          \param idx index of the desired axis
      */
!     salt::Vector3f GetAxis(EAxisIndex idx) const;
  
      /** returns one of the axis angles in degrees, measured between
***************
*** 65,72 ****
          environment.
      */
!     float GetAngle(EAxisIndex idx);
  
      /** returns the time derivate of one of the hinge angles */
!     float GetAngleRate(EAxisIndex idx);
  
      /** sets a joint parameter value */
--- 65,72 ----
          environment.
      */
!     float GetAngle(EAxisIndex idx) const;
  
      /** returns the time derivate of one of the hinge angles */
!     float GetAngleRate(EAxisIndex idx) const;
  
      /** sets a joint parameter value */

Index: body.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/body.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** body.h	31 Mar 2007 13:19:56 -0000	1.4
--- body.h	15 Jun 2007 08:48:17 -0000	1.5
***************
*** 91,94 ****
--- 91,100 ----
      void SetSphere(float density, float radius);
  
+     /** adds a mass representing a sphere of the given radius and
+         density, with the matrix determining its center and
+         orientation
+     */
+     void Addphere(float density, float radius, const salt::Matrix& matrix);
+ 
      /** sets the mass parameters to represent a sphere of the given
          radius and total mass, with the center of mass at (0,0,0)
***************
*** 97,100 ****
--- 103,111 ----
      void SetSphereTotal(float total_mass, float radius);
  
+     /** add a mass representing a sphere of the given radius and total
+         mass, with the matrix determining its center and orientation
+     */
+     void AddSphereTotal(float total_mass, float radius, const salt::Matrix& matrix);
+ 
      /** Set the mass parameters to represent a box of the given
          dimensions and density, with the center of mass at (0,0,0)
***************
*** 103,106 ****
--- 114,123 ----
      void SetBox(float density, const salt::Vector3f& size);
  
+     /** Add a mass representing a box of the given dimensions and
+         density, with the matrix determining its center and
+         orientation
+     */
+     void AddBox(float density, const salt::Vector3f& size, const salt::Matrix& matrix);
+ 
      /** Set the mass parameters to represent a box of the given
          dimensions and total mass, with the center of mass at (0,0,0)
***************
*** 109,112 ****
--- 126,135 ----
      void SetBoxTotal(float total_mass, const salt::Vector3f& size);
  
+     /** Add a mass representing a box of the given dimensions and
+         total mass, with the matrix determining its center and
+         orientation
+     */
+     void AddBoxTotal(float total_mass, const salt::Vector3f& size, const salt::Matrix& matrix);
+ 
      /** Set the mass parameters to represent a flat-ended cylinder of
          the given parameters and density, with the center of mass at
***************
*** 115,119 ****
          long axis is oriented along the body's z axis.
       */
!     void SetCylinder (float density, float radius, float length);
  
      /** Set the mass parameters to represent a flat-ended cylinder of
--- 138,148 ----
          long axis is oriented along the body's z axis.
       */
!     void SetCylinder(float density, float radius, float length);
! 
!     /** Add a mass representing a flat-ended cylinder of the given
!         parameters and density, with the matrix determining its center
!         and orientation
!      */
!     void AddCylinder(float density, float radius, float length, const salt::Matrix& matrix);
  
      /** Set the mass parameters to represent a flat-ended cylinder of
***************
*** 125,128 ****
--- 154,163 ----
      void SetCylinderTotal(float total_mass, float radius, float length);
  
+     /** Add a mass representing a flat-ended cylinder of the given
+         parameters and total mass, with the matrix determining its
+         center and orientation
+      */
+     void AddCylinderTotal(float total_mass, float radius, float length, const salt::Matrix& matrix);
+ 
      /* Set the mass parameters to represent a capped cylinder of the
         given parameters and density, with the center of mass at
***************
*** 134,137 ****
--- 169,178 ----
      void SetCappedCylinder (float density, float radius, float length);
  
+     /* Add a mass representing a capped cylinder of the given
+        parameters and density, with the matrix determining its center
+        and orientation
+     */
+     void AddCappedCylinder (float density, float radius, float length, const salt::Matrix& matrix);
+ 
      /* Set the mass parameters to represent a capped cylinder of the
         given parameters and total mass, with the center of mass at
***************
*** 143,146 ****
--- 184,193 ----
      void SetCappedCylinderTotal(float total_mass, float radius, float length);
  
+     /* Add a mass representing a capped cylinder of the given
+        parameters and total mass, with the matrix determining its
+        center and orientation
+     */
+     void AddCappedCylinderTotal(float total_mass, float radius, float length, const salt::Matrix& matrix);
+ 
      /** displace the mass center relative to the body frame */
      void TranslateMass(const salt::Vector3f& v);
***************
*** 184,187 ****
--- 231,241 ----
      void SynchronizeParent() const;
  
+     /** adds the given ode mass to this body. The given matrix is
+         applied to the mass center
+     */
+     void AddMass(const dMass& mass, const salt::Matrix& matrix);
+ 
+     salt::Vector3f GetMassCenter() const;
+ 
  protected:
      /** creates the managed ODE body and moves it to the position of
***************
*** 193,196 ****
--- 247,304 ----
      bool CreateBody();
  
+     /** sets up an ode mass struct representing a box of the given
+         size and total_mass
+     */
+     void PrepareBoxTotal(dMass& mass, float total_mass, const salt::Vector3f& size) const;
+ 
+     /** sets up an ode mass struct representing a box of the given
+         density and size
+     */
+     void PrepareBox(dMass& mass, float density, const salt::Vector3f& size) const;
+ 
+     /** sets up an ode mass struct representing a sphere of the given
+         density and radius
+     */
+     void PrepareSphere(dMass& mass, float density, float radius) const;
+ 
+     /** sets up an ode mass struct representing a sphere of the given
+         radius and total_mass
+     */
+     void PrepareSphereTotal(dMass& mass, float total_mass, float radius) const;
+ 
+     /** sets up an ode mass struct representing a flat-ended cylinder
+         of the given parameters and density, with the center of mass
+         at (0,0,0) relative to the body. The radius of the cylinder is
+         radius. The length of the cylinder is length. The cylinder's
+         long axis is oriented along the body's z axis.
+      */
+     void PrepareCylinder (dMass& mass, float density, float radius, float length) const;
+ 
+     /** sets up an ode mass struct representing a flat-ended cylinder
+         of the given parameters and total mass, with the center of
+         mass at (0,0,0) relative to the body. The radius of the
+         cylinder is radius. The length of the cylinder is length. The
+         cylinder's long axis is oriented along the body's z axis.
+      */
+     void PrepareCylinderTotal(dMass& mass, float total_mass, float radius, float length) const;
+ 
+     /* sets up an ode mass struct representing a capped cylinder of
+        the given parameters and density, with the center of mass at
+        (0,0,0) relative to the body. The radius of the cylinder (and
+        the spherical cap) is radius. The length of the cylinder (not
+        counting the spherical cap) is length. The cylinder's long axis
+        is oriented along the body's z axis.
+     */
+     void PrepareCappedCylinder (dMass& mass, float density, float radius, float length) const;
+ 
+     /* sets up an ode mass struct representing a capped cylinder of
+        the given parameters and total mass, with the center of mass at
+        (0,0,0) relative to the body. The radius of the cylinder (and
+        the spherical cap) is radius. The length of the cylinder (not
+        counting the spherical cap) is length. The cylinder's long axis
+        is oriented along the body's z axis.
+     */
+     void PrepareCappedCylinderTotal(dMass& mass, float total_mass, float radius, float length) const;
+ 
  private:
      /** updates the the internal state after physics calculation,

Index: collider.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/collider.cpp,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** collider.cpp	1 May 2007 13:09:32 -0000	1.4
--- collider.cpp	15 Jun 2007 08:48:17 -0000	1.5
***************
*** 222,225 ****
--- 222,231 ----
  }
  
+ Vector3f Collider::GetPosition() const
+ {
+     const dReal* pos = dGeomGetPosition(mODEGeom);
+     return Vector3f(pos[0],pos[1],pos[2]);
+ }
+ 
  dSpaceID Collider::GetParentSpaceID()
  {

Index: universaljoint.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/universaljoint.cpp,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** universaljoint.cpp	12 Feb 2006 11:36:52 -0000	1.3
--- universaljoint.cpp	15 Jun 2007 08:48:17 -0000	1.4
***************
*** 79,83 ****
  }
  
! void UniversalJoint::SetAxis1(Vector3f & axis)
  {
      Vector3f first(GetWorldTransform().Rotate(axis));
--- 79,83 ----
  }
  
! void UniversalJoint::SetAxis1(const Vector3f & axis)
  {
      Vector3f first(GetWorldTransform().Rotate(axis));
***************
*** 85,89 ****
  }
  
! void UniversalJoint::SetAxis2(Vector3f & axis)
  {
      Vector3f second(GetWorldTransform().Rotate(axis));
--- 85,89 ----
  }
  
! void UniversalJoint::SetAxis2(const Vector3f & axis)
  {
      Vector3f second(GetWorldTransform().Rotate(axis));
***************
*** 91,95 ****
  }
  
! Vector3f UniversalJoint::GetAxis(EAxisIndex idx)
  {
      Vector3f vec(0,0,0);
--- 91,95 ----
  }
  
! Vector3f UniversalJoint::GetAxis(EAxisIndex idx) const
  {
      Vector3f vec(0,0,0);
***************
*** 118,122 ****
  }
  
! float UniversalJoint::GetAngle(EAxisIndex idx)
  {
      switch (idx)
--- 118,122 ----
  }
  
! float UniversalJoint::GetAngle(EAxisIndex idx) const
  {
      switch (idx)
***************
*** 133,137 ****
  }
  
! float UniversalJoint::GetAngleRate(EAxisIndex idx)
  {
      switch (idx)
--- 133,137 ----
  }
  
! float UniversalJoint::GetAngleRate(EAxisIndex idx) const
  {
      switch (idx)

Index: hingejoint.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/hingejoint.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -C2 -d -r1.5 -r1.6
*** hingejoint.h	19 Feb 2006 13:15:26 -0000	1.5
--- hingejoint.h	15 Jun 2007 08:48:17 -0000	1.6
***************
*** 63,70 ****
          bodies, or between the body and the static environment.
      */
!     float GetAngle();
  
      /** returns the time derivate of the hinge angle */
!     float GetAngleRate();
  
      /** sets a joint parameter value */
--- 63,70 ----
          bodies, or between the body and the static environment.
      */
!     float GetAngle() const;
  
      /** returns the time derivate of the hinge angle */
!     float GetAngleRate() const;
  
      /** sets a joint parameter value */

Index: collider.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/collider.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** collider.h	1 May 2007 13:09:32 -0000	1.4
--- collider.h	15 Jun 2007 08:48:17 -0000	1.5
***************
*** 101,104 ****
--- 101,107 ----
      virtual void SetPosition(const salt::Vector3f& pos);
  
+     /** returns the absolute position of the managed geom */
+     salt::Vector3f GetPosition() const;
+ 
      /** sets the relative orientation of the managed geom directly. If
          the geom is connected to a body, the orientation of the body