[simspark-cvs] simspark/spark/oxygen/physicsserver body.cpp, 1.2.2.1, 1.2.2.1.2.1

[Oliver O. <fr...@us...>]

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

Modified Files:
      Tag: projectx
	body.cpp 
Log Message:
merge from HEAD


Index: body.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/oxygen/physicsserver/body.cpp,v
retrieving revision 1.2.2.1
retrieving revision 1.2.2.1.2.1
diff -C2 -d -r1.2.2.1 -r1.2.2.1.2.1
*** body.cpp	15 Feb 2007 21:00:18 -0000	1.2.2.1
--- body.cpp	17 Jun 2007 08:16:06 -0000	1.2.2.1.2.1
***************
*** 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
  {
***************
*** 264,271 ****
  }
  
! void Body::PostPhysicsUpdateInternal()
  {
-     // synchronize parent node with the bodies position and
-     // orientation
      const dReal* pos = dBodyGetPosition(mODEBody);
      const dReal* rot = dBodyGetRotation(mODEBody);
--- 375,380 ----
  }
  
! void Body::SynchronizeParent() const
  {
      const dReal* pos = dBodyGetPosition(mODEBody);
      const dReal* rot = dBodyGetRotation(mODEBody);
***************
*** 295,298 ****
--- 404,412 ----
  }
  
+ void Body::PostPhysicsUpdateInternal()
+ {
+     SynchronizeParent();
+ }
+ 
  shared_ptr<Body> Body::GetBody(dBodyID id)
  {
***************
*** 352,356 ****
  }
  
! void Body::TranslateMass(const salt::Vector3f& v)
  {
      dMass m;
--- 466,470 ----
  }
  
! void Body::TranslateMass(const Vector3f& v)
  {
      dMass m;