C++ (Cpp) linvel Examples

Example #1

File: physics_score.cpp Project: pbattaglia/physicsvision

int compute_speed(SBullet* bullet, SSimulation simParam, double* scores)
{
  int i, j, k;
  int idx;
  int nObj = bullet->dynamicsWorld->getNumCollisionObjects() - 1;

  /// Do some simulation
  for (i=0;i<simParam.recordIdx[simParam.nRecordTimes - 1] + 1;i++) {
    bullet->dynamicsWorld->stepSimulation(btScalar(simParam.stepSize), 10);

    bool f_record = false;
    for (k=0; k<simParam.nRecordTimes; k++) {
      if (i == simParam.recordIdx[k]) {
	f_record = true;
	break;
      }
    }

    // Records linear and angular velocities, if necessary
    if (f_record) {
      for (j=0; j<nObj; j++) {
	btCollisionObject* obj = 
	  bullet->dynamicsWorld->getCollisionObjectArray()[j + 1];
	btRigidBody* body = btRigidBody::upcast(obj);

	if (body && body->getMotionState()) {
	  btVector3 linvel(body->getLinearVelocity());
	  btVector3 angvel(body->getAngularVelocity());

	  idx = k * nObj * 2 + j * 2;
	  scores[idx] = double(linvel.length());
	  scores[idx + 1] = double(angvel.length());
	}
      }
    }



    // //print positions of all objects (good for debugging)
    // for (j=0; j<nObj; j++) {
    //   btCollisionObject* obj = 
    // 	bullet->dynamicsWorld->getCollisionObjectArray()[j + 1];
    //   btRigidBody* body = btRigidBody::upcast(obj);

    //   if (body && body->getMotionState()) {
    // 	btVector3 linvel(body->getLinearVelocity());
    // 	btVector3 angvel(body->getAngularVelocity());

    // 	// Printout stuff (for debugging)
    // 	if (j >= 0) {
    // 	  btTransform trans;
    // 	  body->getMotionState()->getWorldTransform(trans);

    // 	  printf("\nT %i O %i:  ", i, j);
    // 	  printf("X %.3f, %.3f, %.3f\t", 
    // 	    	 float(trans.getOrigin().getX()), 
    // 	    	 float(trans.getOrigin().getY()), 
    // 	    	 float(trans.getOrigin().getZ()));


    // 	  printf("vL %.3f, %.3f, %.3f |%.3f|\t", 
    // 		 float(linvel.x()), 
    // 		 float(linvel.y()),
    // 		 float(linvel.z()),
    // 		 float(linvel.length()));

    // 	  // printf("aV %.3f, %.3f, %.3f |%.3f|\t", 
    // 	  // 	 float(angvel.x()), 
    // 	  // 	 float(angvel.y()),
    // 	  // 	 float(angvel.z()),
    // 	  // 	 float(angvel.length())
    // 	  // 	 );
    // 	}     
    //   }
    // }




  }
  return 0;
}

Example #2

bool KinematicIntegerBands::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship,
    const TrafficState& repac, int epsv) const {
  // repac is not NULL at this point and k >= 0
  if (k==0) {
    return true;
  }
  std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir);
  Vect3 so = sovo.first;
  Vect3 vo = sovo.second;
  Vect3 si = repac.get_s();
  Vect3 vi = repac.get_v();
  bool rep = true;
  if (k==1) {
    rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv);
  }
  if (rep) {
    std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir);
    Vect3 sot = sovot.first;
    Vect3 vot = sovot.second;
    Vect3 sit = vi.ScalAdd(k*tstep,si);
    Vect3 st = sot.Sub(sit);
    Vect3 vop = linvel(ownship,tstep,trajdir,k-1);
    Vect3 vok = linvel(ownship,tstep,trajdir,k);
    return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) &&
        CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) &&
        CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv);
  }
  return false;
}