[simspark-cvs] simspark/spark/oxygen/physicsserver body.cpp, 1.4, 1.4.4.1 body.h, 1.4, 1.4.4.1 collider.cpp, 1.4, 1.4.2.1 collider.h, 1.4, 1.4.2.1

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

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

Modified Files:
      Tag: ROSIMPORTER_XLAB
	body.cpp body.h collider.cpp collider.h 
Log Message:


Index: body.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/body.h,v
retrieving revision 1.4
retrieving revision 1.4.4.1
diff -C2 -d -r1.4 -r1.4.4.1
*** body.h	31 Mar 2007 13:19:56 -0000	1.4
--- body.h	20 May 2007 16:31:53 -0000	1.4.4.1
***************
*** 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,
***************
*** 206,209 ****
--- 314,319 ----
      /** the managed ode body */
      dBodyID mODEBody;
+ 
+     dMass mMassCenter;
  };
  

Index: collider.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/collider.cpp,v
retrieving revision 1.4
retrieving revision 1.4.2.1
diff -C2 -d -r1.4 -r1.4.2.1
*** collider.cpp	1 May 2007 13:09:32 -0000	1.4
--- collider.cpp	20 May 2007 16:31:53 -0000	1.4.2.1
***************
*** 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: collider.h
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/collider.h,v
retrieving revision 1.4
retrieving revision 1.4.2.1
diff -C2 -d -r1.4 -r1.4.2.1
*** collider.h	1 May 2007 13:09:32 -0000	1.4
--- collider.h	20 May 2007 16:31:53 -0000	1.4.2.1
***************
*** 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

Index: body.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/body.cpp,v
retrieving revision 1.4
retrieving revision 1.4.4.1
diff -C2 -d -r1.4 -r1.4.4.1
*** body.cpp	31 Mar 2007 13:19:56 -0000	1.4
--- body.cpp	20 May 2007 16:31:53 -0000	1.4.4.1
***************
*** 31,34 ****
--- 31,35 ----
  Body::Body() : ODEObject(), mODEBody(0)
  {
+     dMassSetZero(&mMassCenter);
  }
  
***************
*** 37,40 ****
--- 38,46 ----
  }
  
+ Vector3f Body::GetMassCenter() const
+ {
+     return Vector3f(mMassCenter.c[0], mMassCenter.c[1], mMassCenter.c[2]);
+ }
+ 
  dBodyID Body::GetODEBody() const
  {
***************
*** 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
  {
--- 165,353 ----
  }
  
+ 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, track the center of mass in mMassCenter
+     */
+     const Vector3f& trans(matrix.Pos());
+     dMassTranslate(&transMass,trans[0],trans[1],trans[2]);
+     dMassAdd(&mMassCenter,(const dMass*)&transMass);
+ }
  
+ 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;
--- 473,477 ----
  }
  
! void Body::TranslateMass(const Vector3f& v)
  {
      dMass m;