---

collision.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 "collision.h"
#include "world.h"
#include "log.h"
#include "agent.h"


/**
 * Default  constructor, but not usefull at all.
 */
Collision::Collision(){
  type=noCollision;
}


/**
 * This collision is against another cylinder. Just copy some data.
 */
Collision::Collision(Cylinder *_cyl1, const Point3d &_pt1, Cylinder *_cyl2, const Point3d &_pt2, Vector3d &_normal, collisionType _type){
  cyl1=_cyl1;
  cyl2=_cyl2;
  pt1=_pt1;
  pt2=_pt2;
  normal=_normal;
  type=_type;
}


/**
 * This collision is always against the ground plane, so the normal is always (0,0,1)
 */
Collision::Collision(Cylinder *_cyl, const Point3d &_pt, collisionType _type){
  cyl1=_cyl;
  cyl2=NULL;
  pt1=_pt;
  type=_type;
  normal=Vector3d(0,0,1);
}

/**
 * Two points of one cylinder are touching in some way a plane, the
 * only difference is that we should anulate moment
 */
Collision::Collision(Cylinder *_cyl, const Point3d &_pt1, const Point3d &_pt2, collisionType _type){
  cyl1=_cyl;
  cyl2=NULL;
  pt1=_pt1;
  pt2=_pt2;
  type=_type;
  normal=Vector3d(0,0,1);
}



Collision::~Collision(){
}

/**
 * Resolves a collision
 */
void Collision::resolve(){
  if (type==noCollision)
    return;

  if (cyl2==NULL)
    resolveGroundCollision();
  else
    resolveCylinderCollision();
}

void Collision::resolveGroundCollision(){
  Quaternion base=cyl1->orientationBase; // Faster access... and cleaner code

  Vector3d relativeVelocity=cyl1->velocity
    +(base.toWorld(cyl1->angularVelocity^pt1));
  
  Vector3d tangent(0,0,0);
  if ((relativeVelocity.X()!=0)&&(0!=relativeVelocity.Y())){
    tangent=Vector3d(relativeVelocity.X(), relativeVelocity.Y(),0); /// A quick way for this case (ground)
    tangent.normalize();
  }

  if (type==contact){
    Vector3d contactForce=Vector3d(0,0,0);
    Vector3d frictionForce=Vector3d(0,0,0);

    Vector3d A=cyl1->inertiaInverse*(cyl1->moment - (cyl1->angularVelocity^(cyl1->inertia*cyl1->angularVelocity)));
    Vector3d relativeAcceleration=(cyl1->force/cyl1->mass) +
      base.toWorld((cyl1->angularVelocity^(cyl1->angularVelocity^pt1)) +
           (A^pt1));
    
    if (relativeAcceleration.Z()<0) /// If accelerate vs. the ground
      contactForce=cyl1->mass*((-relativeAcceleration*normal)*normal);
    if (tangent!=Vector3d(0,0,0)){    /// Maybe there is also friction force
      frictionForce=(contactForce.module()*World::getGroundFrictionCoefficient())*tangent;
    }


    // Then I apply the forces
    cyl1->force+=contactForce+frictionForce;
    
    /// I convert from world force coordinates, to body
    contactForce=base.toBody(contactForce);
    frictionForce=base.toBody(frictionForce);
    /// and set the moment
    cyl1->moment+=pt1^(contactForce+frictionForce);
  }

  if (relativeVelocity.Z()<COLLISION_VELOCITY_EPSILUM){
#define  mu World::getGroundFrictionCoefficient()
#define  Cr World::getGroundRestitutionCoefficient()
    Point3d pt1w=base.toWorld(pt1);

    /// We calculate the impulse, this case is for the ground
    double j=(-(1+Cr)*(relativeVelocity.Z())) /
      ( (1/cyl1->mass) + (normal*(((pt1w^normal)*cyl1->inertiaInverse)^pt1w)) );

    j=fabs(j);

    if (j<0){
      log <<"J LESS THAN 0!!!"<<j<<endl;
      log <<relativeVelocity.toString()<<endl;
      log <<cyl1->velocity.toString()<<endl;
      cyl1->velocity=Vector3d(0,0,0);
      cyl1->angularVelocity=Vector3d(0,0,0);
      return;
    }
    if ((j>1e3)||(j<0))
      j=1e3;

    Vector3d impulse;
    if (fabs(relativeVelocity*tangent) > 0.0)
      impulse=(j*normal) + ((mu*j)*tangent);
    else
      impulse=j*normal;

    cyl1->velocity+=impulse/cyl1->mass;
    cyl1->angularVelocity+=( pt1^base.toBody(impulse) ) * cyl1->inertiaInverse;
  }
}


/**
 * Resolves the forces that have to applied against both cylinders, the
 * moments, and all those things, depending if they are in contact, o
 * penetrating...
 */
void Collision::resolveCylinderCollision(){
  //log <<"Cylinder collision!!"<<endl;
}

---