---

cylinder_physics.cc

Go to the documentation of this file.

/*
  svas_server -- virtual World Server of Svas
  Copyright (c) 2001, 2002 David Moreno Montero
 
 
  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.
 
  This program is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.
 
  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
  02111-1307, USA.  

*/

#include <thread.h>
#include <math.h>
#include "cylinder.h"
#include "log.h"
#include "world.h"
#include "agent.h"
#include "collision.h"

/**
 * Updates the moment of inertia of the cylinder
 */
void Cylinder::updateMomentOfInertia(){
  double xxyy=((1.0/12.0)*mass*height*height) + ((1.0/4.0)*mass*width*width);
  double zz=(1.0/2.0)*mass*width*width;

  inertia=Matrix3x3(xxyy,0,0,
            0,xxyy,0,
            0,0,zz);
  inertiaInverse=inertia.Inverse();
}



/**
 * As a new cycle begins, we need to reset the forces, moments... It
 * also adds the forces due to gravity, linear drag and angular drag.
 **/
void Cylinder::newCycle(){
  unsigned int i;
  updateMomentOfInertia();

  if (fabs(velocity.X())<COLLISION_VELOCITY_EPSILUM)    velocity=Vector3d(0,velocity.Y(),velocity.Z());
  if (fabs(velocity.Y())<COLLISION_VELOCITY_EPSILUM)    velocity=Vector3d(velocity.X(),0,velocity.Z());
  if (fabs(velocity.Z())<COLLISION_VELOCITY_EPSILUM)    velocity=Vector3d(velocity.X(),velocity.Y(),0);

  if (fabs(angularVelocity.X())<COLLISION_ANGULARVELOCITY_EPSILUM)    angularVelocity=Vector3d(0,angularVelocity.Y(),angularVelocity.Z());
  if (fabs(angularVelocity.Y())<COLLISION_ANGULARVELOCITY_EPSILUM)    angularVelocity=Vector3d(angularVelocity.X(),0,angularVelocity.Z());
  if (fabs(angularVelocity.Z())<COLLISION_ANGULARVELOCITY_EPSILUM)    angularVelocity=Vector3d(angularVelocity.X(),angularVelocity.Y(),0);

  /// I update sons glueposition...
  for(i=son.size();i--;){
    son[i]->gluePosition=(orientationWorld*son[i]->gluePositionBody.X()*height) + 
      (son[i]->orientationWorld*radius);
  }

  force=Vector3d(0,0,0);
  oldForce=force;
  moment=Vector3d(0,0,0);
  oldMoment=moment;

  /// Gravity
  force+=Vector3d(0,0,1)*mass*World::getGravity();

  /// Linear Drag
  /// The last part (width*height) is a very bad approximation of area
  /// exposed to the drag force
  double speed=velocity.module();
  if (speed!=0.0)
    force+=((-velocity).normalized())*(speed*speed*World::getLinearDragCoefficient()*width*height);

  /// Angular Drag
  double aspeed=angularVelocity.module();
  if (aspeed!=0.0)
    moment+=((-angularVelocity).normalized())*(aspeed*aspeed*World::getAngularDragCoefficient()*width*height);

  /// Spring and damper forces
  double f;
  double l;
  Vector3d L,V;
  Vector3d O;
  
  for (i=son.size();i--;){
    L=son[i]->gluePosition-son[i]->position; //son[i]->gluePosition-(son[i]->position+velocity+(son[i]->force/son[i]->mass));
    l=L.module();
    if (l==0)
      continue;
    if (l>length){
      drop(son[i]->getId(),NULL,0);
      continue;
    }
    V=-(velocity-son[i]->velocity);
    f=(World::getSpringCoefficient()*l+
       World::getDamperCoefficient()*(V*L)/l); // The force scalar

    O=Vector3d(sin(son[i]->gluePositionBody.Y()),
           cos(son[i]->gluePositionBody.Y()),0);

    //log <<O.toString()<<endl;

    /// Action   - My force against my son
    //    applyForceOnBody(O*f,(O*radius)+Vector3d(0,0,(son[i]->gluePositionBody.X()-0.5)*son[i]->length/2));
    moment+=(O*f)^((O*radius)+Vector3d(0,0,(son[i]->gluePositionBody.X()-0.5)*son[i]->length));

    /// Reaction!
    //    son[i]->applyForceOnBody(Vector3d(0,0,-f),Vector3d(0,0,-length/2));
    son[i]->moment+=Vector3d(0,0,-f)^(Vector3d(0,0,-length/2));

    son[i]->setToGluePosition();
  }
  
  /// Check contact force with the ground
  Collision *col=checkGroundCollision();
  if (NULL!=col){
    if (col->getType()==contact)
      col->resolve();
    delete col;
  }


  /*
  for (i=son.size();i--;){
    son[i]->setToGluePosition();
  }
  */
}

/**
 * Moves the cylinder to the glue position, if any
 */
void Cylinder::setToGluePosition(void){
  if (NULL!=father)
    position=gluePosition;
}

/**
 * Moves the position of the cylinder, but with care with parent; in
 * case the cylinder have a parent, it moves the parent too. Have care
 * about gluePosition too.
 */
void Cylinder::movePosition(const Vector3d &ammount){
  /*
  if (father!=NULL)
    father->movePosition(ammount);
  else
  */
  position+=ammount;
}

/**
 * Apply a force over the cylinder, causing linear and angular acceleration
 *
 * the point where the force is applied is perpendicular to the
 * axis. (through the line that connects the axis and the point where)
 */
void Cylinder::applyForce(const Vector3d &_force, const Point3d &where){
  log <<"Do not use this force!"<<endl;
  force+=_force;
  //  moment+= _force^((~orientation).rotate(where-getCentreOfGravity()).getAxis());
}

/**
 * Apply a force over the cylinder, causing linear and angular acceleration
 *
 * The force and point are both in body coordinates
 */
void Cylinder::applyForceOnBody(const Vector3d &_force, const Point3d &where){
  force+=orientationBase.toWorld(_force);
  moment+= _force^where;
}


/**
 * Move the cylinders, and check collissions. Returns true if it's
 * bellow the floor
 */
void Cylinder::applyVelocity(double dtime){
  /// first we save status
  oldPosition=position;
  oldOrientationBase=orientationBase;
  oldVelocity=velocity;
  oldAngularVelocity=angularVelocity;
  oldForce=force;
  oldMoment=moment;

  ////// And now apply velocity
  // Linear Velocity
  Vector3d A=force/mass;
  velocity+=A*dtime;
  position+=velocity*dtime;

  // Angular velocity
  A=inertiaInverse*(moment - (angularVelocity^(inertia*angularVelocity)));

  angularVelocity+=A*dtime;
  orientationBase+=(orientationBase*angularVelocity) * (0.5 *dtime);
  orientationBase.normalize();

  position+=orientationWorld*(length/2); // I set position to the center

  orientationWorld=orientationBase.toWorld(Vector3d(0,0,1));
  orientationWorld.normalize();

  position-=orientationWorld*(length/2); // and back to the position


  /*    
  unsigned int i;
  for (i=son.size();i--;)
    son[i]->orientationBase=(orientationBase*son[i]->orientationBase).normalized();
  */
}

/**
 * Undoes latest velocity, setting the oldXXX values that were saved
 */
void Cylinder::undoVelocity(void){
  position=oldPosition;

  orientationBase=oldOrientationBase;
  orientationWorld=orientationBase.toWorld(Vector3d(0,0,1));
  orientationWorld.normalize();

  velocity=oldVelocity;
  angularVelocity=oldAngularVelocity;
  force=oldForce;
  moment=oldMoment;
}


/**
 * Checks all collisions; against other cylinders, ground...
 */
vector<Collision *> Cylinder::checkCollisions(){
  Collision *coll;
  vector<Collision *> ret;
  
  /// against ground
  coll=checkGroundCollision();
  if (NULL!=coll)
    ret.push_back(coll);

  /// against other cylinders
  unsigned int i;
  Cylinder *c;
  for (i=world->getNumCylinders();i--;){
    c=world->getCylinder(i);
    if (c!=this)
      if ((coll=checkCylinderCollision(c))!=NULL)
    ret.push_back(coll);
  }

  return ret;
}

/**
 * Check the collision against the ground
 *
 * The way it works is: 
 *
 * The cylinder is defined by two planes at position (p1) and at
 * position+orientation*height (p2) (the two extremes) and the
 * radius (width).
 *
 * 1.We check that p1 and p2 are above the plane, else they are in
 * contact, or more probably penetrating
 *
 * 2.then we check the lines that are the cut's of both planes to the
 * ground (l1 and l2), and the distance from the lines l1 and l2 to p1
 * and p2, respectively, if it's less than the radius, then the
 * cylinder cuts the plane. We check also for contact.
 */
Collision *Cylinder::checkGroundCollision(){
  Point3d p1,p2, p0, pb1, pb2,pb0;
  Vector3d o=orientationWorld;


  p1=getPosition();
  p2=p1+(o*height);
  p0=p1+(o*(height/2));
  
  pb1=Vector3d(0,0,-length/2);
  pb2=Vector3d(0,0,+length/2);
  pb0=Vector3d(0,0,0);


  /// If any of the points is bellow, they are, for sure, penetrating
  if ((p1.Z()<-COLLISION_SPACE_EPSILUM)&&(p2.Z()<-COLLISION_SPACE_EPSILUM)){
    if (p1.Z()<p2.Z()){
      if ((velocity.Z()+orientationBase.toWorld(angularVelocity^pb1).Z())<COLLISION_VELOCITY_EPSILUM)
    return new Collision(this, pb1, penetrating);
    }
    else{
      if ((velocity.Z()+orientationBase.toWorld(angularVelocity^pb2).Z())<COLLISION_VELOCITY_EPSILUM)
    return new Collision(this, pb2, penetrating);
    }
  }
  if (p0.Z()<-COLLISION_SPACE_EPSILUM) // or only one
    if (velocity.Z()<COLLISION_VELOCITY_EPSILUM)
      return new Collision(this, pb0, penetrating);
  if (p1.Z()<-COLLISION_SPACE_EPSILUM) 
    if ((velocity.Z()+orientationBase.toWorld(angularVelocity^pb1).Z())<COLLISION_VELOCITY_EPSILUM)
      return new Collision(this, pb1, penetrating);
  if (p2.Z()<-COLLISION_SPACE_EPSILUM)
    if ((velocity.Z()+orientationBase.toWorld(angularVelocity^pb2).Z())<COLLISION_VELOCITY_EPSILUM)
      return new Collision(this, pb2, penetrating);


  /// If it's between -COL and +COL, they are in contact
  if (force.Z()<0){ /// Only if I'm accelerating vs the ground, and
            /// the point that goes against it
    if ((p1.Z()<COLLISION_SPACE_EPSILUM)&&(p2.Z()<COLLISION_SPACE_EPSILUM)){
      if (p1.Z()<p2.Z())
    return new Collision(this, pb1, contact);
      else
    return new Collision(this, pb2, contact);
    }
    if (p0.Z()<COLLISION_SPACE_EPSILUM) // or only one
      return new Collision(this, pb0, contact);
    if (p1.Z()<COLLISION_SPACE_EPSILUM)
      return new Collision(this, pb1, contact);
    if (p2.Z()<COLLISION_SPACE_EPSILUM)
      return new Collision(this, pb2, contact);
  }

  /// Greater than +COL... no collision
  return NULL;
}

/**
 * Check a collision against another cylinder. 

 * The way to calculate this is to first check the distance between
 * the two centers of gravity; if it's grater then the sum of both
 * lengths then sure they do not collide; then I check the distance
 * from both lines (each from one cylinder point, and with the
 * orientation).
 */
Collision *Cylinder::checkCylinderCollision(Cylinder *other){
  return NULL;
  /*
  double dcc=(other->getCentreOfGravity()-getCentreOfGravity()).module(); /// Distance Center to Center
  double srr=(other->length+length);  /// Sum of Radius Radius
  if ( dcc > srr+2*COLLISION_SPACE_EPSILUM)
    return NULL;

  if ((orientationWorld^other->orientationWorld).module()==0){ // Parallel lines... just check distance...
    double d=(orientationWorld^(position-other->position)).module();
    d=(d-srr)/2;
    if (d<-COLLISION_SPACE_EPSILUM)
      return new Collision(this, other, other->getCentreOfGravity()+getCentreOfGravity()/2, penetrating);
    else if (d<COLLISION_SPACE_EPSILUM)
      return new Collision(this, other, other->getCentreOfGravity()+getCentreOfGravity()/2, contact);
    else 
      return NULL;
  }
  // Not parallel...
  /// Check distance between two lines!


  
  //new Collision(this, other, other->getCentreOfGravity()+getCentreOfGravity()/2, contact);
  

  return NULL;*/
}

---