oasisphysics.cpp

使用stl技术,(还没看,是听说的)

    }
    
    if ( !dynamicsEnabled ) {
      setDynamicsEnabled( true );
    }

    body->setLinearVel( velocity.x, velocity.y, velocity.z );    
  }

  const vector3 &physics::getLinearVelocity( void ) const {
    if ( !drivingPhysical ) {
      oasisError( exception::ET_INTERNAL_ERROR,
      "Cannot get velocity on an unused physics",
      "physics::setLinearVelocity" );
    }

    static vector3 vel;
    vel = convert::parseVector3( body->getLinearVel( ) );
    return vel;
  }
    
  void physics::setAngularVelocity( real x, real y, real z ) {
    setAngularVelocity( vector3( x, y, z ) );
  }

  void physics::setAngularVelocity( const vector3 &velocity ) {
    if ( !drivingPhysical ) {
      oasisError( exception::ET_INTERNAL_ERROR,
      "Cannot set velocity on an unused physics",
      "physics::setAngularVelocity" );
    }
    
    if ( !dynamicsEnabled ) {
      setDynamicsEnabled( true );
    }

    body->setAngularVel( velocity.x, velocity.y, velocity.z );    
  }

  const vector3 &physics::getAngularVelocity( void ) const {
    if ( !drivingPhysical ) {
      oasisError( exception::ET_INTERNAL_ERROR,
      "Cannot get velocity on an unused physics",
      "physics::setAngularVelocity" );
    }

    static vector3 vel;
    vel = convert::parseVector3( body->getAngularVel( ) );
    return vel;
  }

  void physics::addForce( real x, real y, real z,
        real rx, real ry, real rz ) {
    addForce( vector3( x, y, z ), vector3( rx, ry, rz ) );
  }

  void physics::addForce( const vector3 &force, const vector3 &position ) {
    body->addForceAtRelPos( force.x, force.y, force.z,
          position.x, position.y, position.z );
  }

  void physics::addForceWorld( real x, real y, real z,
             real rx, real ry, real rz ) {
    addForceWorld( vector3( x, y, z ), vector3( rx, ry, rz ) );
  }

  void physics::addForceWorld( const vector3 &force,
             const vector3 &worldPos ) {
    body->addForceAtPos( force.x, force.y, force.z,
       worldPos.x, worldPos.y, worldPos.z );
  }

  void physics::setPosition( const vector3 &newPosition ) {
    body->setPosition( newPosition.x, newPosition.y, newPosition.z );
  
    updateProxies( );
  }

  void physics::setOrientation( const quaternion &newOrientation ) {
    dQuaternion quat;
    convert::toODEQuaternion( quat, newOrientation );
    body->setQuaternion( quat );
   
    updateProxies( );
  }

  void physics::addTorque( real x, real y, real z ) {
    addTorque( vector3( x, y, z ) );
  }

  void physics::addTorque( const vector3 &torque ) {
    body->addRelTorque( torque.x, torque.y, torque.z );
  }

  void physics::addTorqueWorld( real x, real y, real z ) {
    addTorqueWorld( vector3( x, y, z ) );
  }

  void physics::addTorqueWorld( const vector3 &torque ) {
    body->addTorque( torque.x, torque.y, torque.z );
  }

  void physics::setPhysical( physical *newPhysical ) {
    drivingPhysical = newPhysical;

    if ( drivingPhysical ) {
      setDynamicsEnabled( true );
      setCollisionEnabled( true );
    } else {
      setDynamicsEnabled( false, false );
      setCollisionEnabled( false );
    }
  }

  dBody *physics::getODEBody( void ) const {
    return body;
  }

  void physics::setup( resource *parent ) {
    physicsResource *pp = ( physicsResource* )parent;
   
    physicsResource::proxyIterator it = pp->getProxyIterator( );
        
    while ( !it.isEmpty( ) ) {
      switch ( it.getCurrent( )->getType( ) ) {
      case proxy::PT_BOX:
  addBoxProxy( 0, 0, 0 )->
    copyParameters( static_cast< boxProxy* >( it.getCurrent( ) ) );
  break;
      case proxy::PT_SPHERE:  
  addSphereProxy( 0 )->
    copyParameters( static_cast< sphereProxy* >( it.getCurrent( ) ) );
  break;
      case proxy::PT_CCYLINDER:
  addCCylinderProxy( 0, 0 )->
    copyParameters( static_cast< ccylinderProxy* >( it.getCurrent( ) ) );
  break;
      };

      it.moveNext( );
    }

    updateProxies( );

    usedMass->copyParameters( pp->getMass( ) );
    
    setGravity( pp->getGravity( ) );   
    setSoftness( pp->getSoftness( ) );
    setBounce( pp->getBounceCoEff( ), pp->getBounceVelThreshold( ) );
    setFriction( pp->getFriction( ) );   
  }

  void physics::clear( void ) {
    setPhysical( NULL );   
    usedMass->setSphere( 0, 0 );   
    removeAllProxies( );
  }
  
  void physics::updateDynamics( real time ) {
    // If the body has been reenabled due to collision, enable this
    if ( reenableDynamics ) {
      dynamicsEnabled = body->isEnabled( ) == 0 ? false : true;
    }
    
    // Update dynamics
    if ( dynamicsEnabled ) {          
      drivingPhysical->
  updatePhysics( convert::parseVector3( body->getPosition( ) ),
           convert::parseQuaternion( body->getQuaternion( ) ) );
    
      updateProxies( );
      
      // See if we should disable this
      if ( ( getLinearVelocity( ).squaredLength( ) <= PHYSICS_THRESHOLD_LINEAR ) &&
     ( getAngularVelocity( ).squaredLength( ) <= PHYSICS_THRESHOLD_ANGULAR ) ) {
  
  if ( disableTimer > PHYSICS_THRESHOLD_TIMER ) {   
    setDynamicsEnabled( false );
    disableTimer = 0;  
  } else {
    disableTimer += time;  
  }
      } else {
  disableTimer = 0;
      }
    }        
  }

  void physics::testCollision( physics *other, dxGeom *one, dxGeom *two ) {
    dContact contact[ PHYSICS_MAX_CONTACTS ];
    collisionListener::CollisionInfo collInfo;
    
    // If either has no collision enabled, there gonna be no point in this
    if ( !collisionEnabled || 
   !other->getCollisionEnabled( ) ) {
      return;
    }

    // If both are not dynamic, there gonna be no point to this either
    if ( !dynamicsEnabled &&
   !other->getDynamicsEnabled( ) ) {
      return;
    }
    
    // Calculate the collision bodies
    // Assume both are static
    dBodyID bod1, bod2;
    bod1 = bod2 = 0;
    
    if ( dynamicsEnabled ||
   other->getDynamicsReenabled( ) ) {      
      bod1 = body->id( );
    }
    
    if ( other->getDynamicsEnabled( ) ||
   other->getDynamicsReenabled( ) ) {
      bod2 = other->getODEBody( )->id( );
    }

    // See if they collide
    uint8 collide = dCollide( one, two, PHYSICS_MAX_CONTACTS,
            &contact[ 0 ].geom, sizeof( dContact ) );
    
    if ( collide ) {   
      // NB!!!
      // This will be tweaked as time goes by to try and achieve the best
      // results
      real mbounce = std::max( bounceCoeffRest,
             other->getBounceCoEff( ) ); 
      real mvel = std::min( bounceVelThreshold,
          other->getBounceVelThreshold( ) );      
      real tsoftness = softness + other->getSoftness( );
      real tfriction = std::min( friction, other->getFriction( ) );
      
      for ( uint8 point = 0; point < collide; point++ ) {  
  contact[ point ].surface.mode = dContactSlip1 | dContactSlip2 | dContactApprox1;
  contact[ point ].surface.bounce = mbounce;
  contact[ point ].surface.bounce_vel = mvel;
  contact[ point ].surface.slip1 = 0.0;
  contact[ point ].surface.slip2 = 0.0;
  
  if ( tsoftness ) {
    contact[ point ].surface.mode |= dContactSoftCFM;
    contact[ point ].surface.soft_cfm = tsoftness;
  }
  
  contact[ point ].surface.mu = tfriction;
  contact[ point ].surface.mu2 = 0;
  
  dContactJoint contactJoint( physicsSystem::get( ).getODEWorld( )->id( ),
            physicsSystem::get( ).getODEContactJointGroup( )->id( ),
            contact + point );
  
  contactJoint.attach( bod1, bod2 );

  // Notify the physicals that there was a collision
  collInfo.position.x = contact[ point ].geom.pos[ 0 ];
  collInfo.position.y = contact[ point ].geom.pos[ 1 ];
  collInfo.position.z = contact[ point ].geom.pos[ 2 ];
  collInfo.normal.x = contact[ point ].geom.normal[ 0 ];
  collInfo.normal.y = contact[ point ].geom.normal[ 1 ];
  collInfo.normal.z = contact[ point ].geom.normal[ 2 ];
  collInfo.depth = contact[ point ].geom.depth;

  collInfo.otherPhysical = other->drivingPhysical;
  drivingPhysical->notifyCollision( collInfo );

  collInfo.otherPhysical = drivingPhysical;
  other->drivingPhysical->notifyCollision( collInfo );
      }
    }  
  }
}