Expression<Wrench>::Ptr Composition_RotationWrench::derivativeExpression(int i) {
    Expression<Rotation>::Ptr a = cached<Rotation>(argument1);    
    Expression<Vector>::Ptr da  = cached<Vector>(argument1->derivativeExpression(i));    
    Expression<Wrench>::Ptr b   = cached<Wrench>(argument2);    
    Expression<Wrench>::Ptr db  = cached<Wrench>(argument2->derivativeExpression(i));    
    return wrench( a*force(db) + da*(a*force(b)),
                   a*torque(db) + da*(a*torque(b)) );
}
/*! This is a helper function for grasp force optimization routines. Given a
	set of contacts, and a matrix of contact wrenches expressed in *world*
	coordinates and relative to object origin (as computed in gfo routines)
	this function will convert them to object coordinate system and accumulate
	them in the object's external wrench accumulator. This can serve as an
	output of the gfo functions and also for rendering purposes, as the 
	external wrench accumulator can then be rendered.
*/
void
Grasp::accumulateAndDisplayObjectWrenches(std::list<Contact*> *contacts, 
										  const Matrix &objectWrenches)
{
	int count = 0;
	std::list<Contact*>::iterator it;
	for (it=contacts->begin(); it!=contacts->end(); it++, count++) {
		Contact *contact = *it;
		vec3 force(objectWrenches.elem(6*count+0,0), 
					objectWrenches.elem(6*count+1,0), 
					objectWrenches.elem(6*count+2,0));
		vec3 torque(objectWrenches.elem(6*count+3,0), 
					objectWrenches.elem(6*count+4,0), 
					objectWrenches.elem(6*count+5,0));
		if (contact->getBody2()->isDynamic()) {
			DynamicBody *object = (DynamicBody*)(contact->getBody2());
			//compute force and torque in body reference frame
			//and scale them down for now for rendering and output purposes
			force = 1.0e-6 * force * object->getTran().inverse();
			//torque is also scaled by maxRadius in conversion matrix
			torque = 1.0e-6 * torque * object->getTran().inverse();
			//accumulate them on object
			object->addForce(force);
			object->addTorque(torque);
		}
	}
}
static void GetCollisionSubShape(const NewtonJoint* const contactJoint, NewtonBody* const body)
{
	NewtonCollisionInfoRecord collisionInfo;

	NewtonCollision* const collision = NewtonBodyGetCollision(body);
	NewtonCollisionGetInfo (collision, &collisionInfo);


	int count = 0;
	NewtonCollision* collidingSubShapeArrar[32];

	// see if this is a compound collision or any other collision with sub collision shapes  
	if (collisionInfo.m_collisionType == SERIALIZE_ID_COMPOUND) {

		// to get the torque we need the center of gravity in global space
		dVector origin;
		dMatrix bodyMatrix;
		NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
		NewtonBodyGetCentreOfMass(body, &origin[0]);
		origin = bodyMatrix.TransformVector(origin);

		for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
			// get the material of this contact, 
			// this part contain all contact information, the sub colliding shape, 
			NewtonMaterial* const material = NewtonContactGetMaterial (contact);
			NewtonCollision* const subShape = NewtonMaterialGetBodyCollidingShape (material, body);
			int i = count - 1;
			for (; i >= 0; i --) {
				if (collidingSubShapeArrar[i] == subShape) {
					break;
				}
			}
			if (i < 0) {
				collidingSubShapeArrar[count] = subShape;
				count ++;
				dAssert (count < int (sizeof (collidingSubShapeArrar) / sizeof (collidingSubShapeArrar[0])));

				// you can also get the forces here, however when tho function is call form a contact material
				// we can only get resting forces, impulsive forces can not be read here since they has no being calculated yet.
				// whoever if this function function is call after the NetwonUpdate they the application can read the contact force, that was applied to each contact point
				dVector force;
				dVector posit;
				dVector normal;
				NewtonMaterialGetContactForce (material, body, &force[0]);
				NewtonMaterialGetContactPositionAndNormal (material, body, &posit[0], &normal[0]);
				// the torque on this contact is
				dVector torque ((origin - posit) * force);

				// do what ever you want wit this  


			}
		}
	}

	// here we should have an array of all colling sub shapes
	if (count) {
		// do what you need with this sub shape list
	}
}
Expression<Wrench>::Ptr RefPoint_WrenchVector::derivativeExpression(int i) {
    Expression<Wrench>::Ptr a   = cached<Wrench>(argument1);    
    Expression<Wrench>::Ptr da  = cached<Wrench>(argument1->derivativeExpression(i));    
    Expression<Vector>::Ptr b   = cached<Vector>(argument2);    
    Expression<Vector>::Ptr db  = cached<Vector>(argument2->derivativeExpression(i));    
        return wrench( force(da),
                       torque(da) + force(da)*b + force(a)*db );
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosForce::UpdateChild()
{
  this->lock_.lock();
  math::Vector3 force(this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z);
  math::Vector3 torque(this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z);
  this->link_->AddForce(force);
  this->link_->AddTorque(torque);
  this->lock_.unlock();
}
// Update the controller
void ThrusterPlugin::updateChild()
{
    math::Vector3 force(0.0,0.0,0.0);
    math::Vector3 torque(0.0,0.0,0.0);
    _baseLink->SetTorque(torque);
    _baseLink->SetForce(force);
    applyThrusterForce();
    applyDragFriction();
}
Ejemplo n.º 7
0
void BulletPhysics::ApplyTorque(const Vec3& dir, float newtons, GameObjectId id)
{
	btRigidBody* body = FindBulletRigidBody(id);
	if (body)
	{
		body->setActivationState(DISABLE_DEACTIVATION);
		btVector3 torque(dir.x * newtons, dir.y * newtons, dir.z * newtons);

		body->applyTorqueImpulse(torque);
	}
}
void dVehicleSingleBody::StatesToRigidBody(dFloat timestep)
{
	dComplementaritySolver::dBodyState* const chassisBody = GetBody();

	dVector force(chassisBody->GetForce());
	dVector torque(chassisBody->GetTorque());
	NewtonBodySetForce(m_newtonBody, &force[0]);
	NewtonBodySetTorque(m_newtonBody, &torque[0]);

	dVehicleInterface::StatesToRigidBody(timestep);
}
void dgCollisionInstance::CalculateBuoyancyAcceleration (const dgMatrix& matrix, const dgVector& origin, const dgVector& gravity, const dgVector& fluidPlane, dgFloat32 fluidDensity, dgFloat32 fluidViscosity, dgVector& unitForce, dgVector& unitTorque)
{
	dgMatrix globalMatrix (m_localMatrix * matrix);

	unitForce = dgVector (dgFloat32 (0.0f));
	unitTorque = dgVector (dgFloat32 (0.0f));
	dgVector volumeIntegral (m_childShape->CalculateVolumeIntegral (globalMatrix, fluidPlane, *this));
	if (volumeIntegral.m_w > dgFloat32 (0.0f)) {
		dgVector buoyanceCenter (volumeIntegral - origin);

		dgVector force (gravity.Scale (-fluidDensity * volumeIntegral.m_w));
		dgVector torque (buoyanceCenter.CrossProduct(force));

		unitForce += force;
		unitTorque += torque;
	}
}
Ejemplo n.º 10
0
static void PhysicsApplyPickForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dVector com;
	dVector veloc;
	dVector omega;
	dMatrix matrix;

	// apply the thew body forces
	if (chainForceCallback) {
		chainForceCallback (body, timestep, threadIndex);
	}

	// add the mouse pick penalty force and torque
	NewtonBodyGetVelocity(body, &veloc[0]);

	NewtonBodyGetOmega(body, &omega[0]);
	NewtonBodyGetVelocity(body, &veloc[0]);
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);

	dVector force (pickedForce.Scale (mass * MOUSE_PICK_STIFFNESS));
	dVector dampForce (veloc.Scale (MOUSE_PICK_DAMP * mass));
	force -= dampForce;

	NewtonBodyGetMatrix(body, &matrix[0][0]);
	NewtonBodyGetCentreOfMass (body, &com[0]);
	
	// calculate local point relative to center of mass
	dVector point (matrix.RotateVector (attachmentPoint - com));
	dVector torque (point * force);

	dVector torqueDamp (omega.Scale (mass * 0.1f));

	NewtonBodyAddForce (body, &force.m_x);
	NewtonBodyAddTorque (body, &torque.m_x);

	// make sure the body is unfrozen, if it is picked
	NewtonBodySetFreezeState (body, 0);
}
void dgCollisionInstance::CalculateBuoyancyAcceleration (const dgMatrix& matrix, const dgVector& origin, const dgVector& gravity, const dgVector& fluidPlane, dgFloat32 fluidDensity, dgFloat32 fluidViscosity, dgVector& accel, dgVector& alpha)
{
	dgMatrix scaledMatrix (m_localMatrix * matrix);

	switch (m_scaleType)
	{
		case m_unit:
		case m_uniform:
		case m_nonUniform:
		{
			scaledMatrix[0] = scaledMatrix[0].Scale4 (m_scale[0]);
			scaledMatrix[1] = scaledMatrix[1].Scale4 (m_scale[1]);
			scaledMatrix[2] = scaledMatrix[2].Scale4 (m_scale[2]);
			break;
		}
		default:
			dgAssert(0);
	}

	accel = dgVector (dgFloat32 (0.0f));
	alpha = dgVector (dgFloat32 (0.0f));
	dgVector volumeIntegral (m_childShape->CalculateVolumeIntegral (scaledMatrix, fluidPlane));
	if (volumeIntegral.m_w > dgFloat32 (0.0f)) {
		dgVector buoyanceCenter (volumeIntegral - origin);

		dgVector force (gravity.Scale3 (-fluidDensity * volumeIntegral.m_w));
		dgVector torque (buoyanceCenter * force);

//		dgFloat32 damp = GetMax (GetMin ((m_veloc % m_veloc) * dgFloat32 (100.0f) * fluidLinearViscousity, dgFloat32 (1.0f)), dgFloat32(dgFloat32 (10.0f)));
//		force -= m_veloc.Scale3 (damp);

		//damp = (m_omega % m_omega) * dgFloat32 (10.0f) * fluidAngularViscousity;
//		damp = GetMax (GetMin ((m_omega % m_omega) * dgFloat32 (1000.0f) * fluidAngularViscousity, dgFloat32(0.25f)), dgFloat32(2.0f));
//		torque -= m_omega.Scale3 (damp);
//		dgThreadHiveScopeLock lock (m_world, &m_criticalSectionLock);
		accel += force;
		alpha += torque;
	}
}
Ejemplo n.º 12
0
		void OnInside(NewtonBody* const visitor)
		{
			dFloat Ixx;
			dFloat Iyy;
			dFloat Izz;
			dFloat mass;
			
			NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz);
			if (mass > 0.0f) {
				dMatrix matrix;
				dVector cog(0.0f);
				dVector accelPerUnitMass(0.0f);
				dVector torquePerUnitMass(0.0f);
				const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f);

				NewtonBodyGetMatrix (visitor, &matrix[0][0]);
				NewtonBodyGetCentreOfMass(visitor, &cog[0]);
				cog = matrix.TransformVector (cog);
				NewtonCollision* const collision = NewtonBodyGetCollision(visitor);

				
				dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision);
				dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume);
				dFloat viscosity = 0.995f;

				NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);

				dVector force (accelPerUnitMass.Scale (mass));
				dVector torque (torquePerUnitMass.Scale (mass));

				dVector omega(0.0f); 
				NewtonBodyGetOmega(visitor, &omega[0]);
				omega = omega.Scale (viscosity);
				NewtonBodySetOmega(visitor, &omega[0]);

				NewtonBodyAddForce (visitor, &force[0]);
				NewtonBodyAddTorque (visitor, &torque[0]);
			}
		}
Ejemplo n.º 13
0
void realrobot::jointCallback(const rt_dynamixel_msgs::JointStateConstPtr msg)
{

    for(int i=0; i<total_dof; i++)
    {
        for (int j=0; j<msg->id.size(); j++)
        {
            if(jointID[i] == msg->id[j])
            {
                q(i) = msg->angle[j];
                if(isFirstBoot)
                {    _desired_q(i) = msg->angle[j]; }

                q_dot(i) = msg->velocity[j];
                torque(i) = msg->current[j];
                jointStateUIPub.msg_.error[i] = msg->updated[j];
            }
        }
    }
    if(isFirstBoot)
    {isFirstBoot = false;}
    _jointRecv = true;
}
void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period)
{
  bool result_written = false;

  if (motor_output_->connected() && motor_output_->enabled()) {
    publisher_motor_command_.publish(motor_output_->getCommand());
    result_written = true;
  }

  if (wrench_output_->connected() && wrench_output_->enabled()) {
    geometry_msgs::WrenchStamped wrench;
    wrench.header.stamp = time;
    wrench.header.frame_id = "base_link";
    wrench.wrench = wrench_output_->getCommand();
    publisher_wrench_command_.publish(wrench);

    if (!result_written) {
      gazebo::math::Vector3 force(wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.force.z);
      gazebo::math::Vector3 torque(wrench.wrench.torque.x, wrench.wrench.torque.y, wrench.wrench.torque.z);
      link_->AddRelativeForce(force);
      link_->AddRelativeTorque(torque - link_->GetInertial()->GetCoG().Cross(force));
    }
  }
}
Ejemplo n.º 15
0
RTC::ReturnCode_t TorqueFilter::onExecute(RTC::UniqueId ec_id)
{
  // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
  static int loop = 0;
  loop ++;

  coil::TimeValue coiltm(coil::gettimeofday());
  RTC::Time tm;
  tm.sec = coiltm.sec();
  tm.nsec = coiltm.usec()*1000;

  if (m_qCurrentIn.isNew()) {
    m_qCurrentIn.read();
  }
  if (m_tauInIn.isNew()) {
    m_tauInIn.read();
  }

  if (m_tauIn.data.length() ==  m_robot->numJoints()) {
    int num_joints = m_robot->numJoints();
    hrp::dvector g_joint_torque(num_joints);
    hrp::dvector torque(num_joints);

    if (m_qCurrent.data.length() ==  m_robot->numJoints()) {
      // reference robot model
      for ( int i = 0; i < m_robot->numJoints(); i++ ){
        m_robot->joint(i)->q = m_qCurrent.data[i];
      }
      m_robot->calcForwardKinematics();
      m_robot->calcCM();
      m_robot->rootLink()->calcSubMassCM();
     
      // calc gravity compensation of each joints
      hrp::Vector3 g(0, 0, 9.8);
      for (int i = 0; i < num_joints; i++) {
        // subm*g x (submwc/subm - p) . R*a
        g_joint_torque[i] = (m_robot->joint(i)->subm*g).cross(m_robot->joint(i)->submwc / m_robot->joint(i)->subm - m_robot->joint(i)->p).dot(m_robot->joint(i)->R * m_robot->joint(i)->a);
      }
    } else {
      if (DEBUGP) {
        std::cerr << "[" <<  m_profile.instance_name << "]" << "input is not correct" << std::endl;
        std::cerr << "[" <<  m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
        std::cerr << "[" <<  m_profile.instance_name << "]" << " qCurrent: " << m_qCurrent.data.length() << std::endl;
        std::cerr << std::endl;
      }
      for (int i = 0; i < num_joints; i++) {
        g_joint_torque[i] = 0.0;
      }
    }

    if (DEBUGP) {
      std::cerr << "[" <<  m_profile.instance_name << "]" << "raw torque: ";
      for (int i = 0; i < num_joints; i++) {
        std::cerr << " " << m_tauIn.data[i] ;
      }
      std::cerr << std::endl;
      std::cerr << "[" <<  m_profile.instance_name << "]" << "gravity compensation: ";
      for (int i = 0; i < num_joints; i++) {
        std::cerr << " " << g_joint_torque[i];
      }
      std::cerr << std::endl;
    }

    for (int i = 0; i < num_joints; i++) {
      // torque calculation from electric current
      // torque[j] = m_tauIn.data[path->joint(j)->jointId] - joint_torque(j);
      // torque[j] = m_filters[path->joint(j)->jointId].executeFilter(m_tauIn.data[path->joint(j)->jointId]) - joint_torque(j); // use filtered tau
      torque[i] = m_filters[i].executeFilter(m_tauIn.data[i]) - m_torque_offset[i];

      // torque calclation from error of joint angle
      // if ( m_error_to_torque_gain[path->joint(j)->jointId] == 0.0
      //      || fabs(joint_error(j)) < m_error_dead_zone[path->joint(j)->jointId] ) {
      //   torque[j] = 0;
      // } else {
      //   torque[j] = error2torque(j, fabs(m_error_dead_zone[path->joint(j)->jointId]));
      // }

      // set output
      // gravity compensation
      if(m_is_gravity_compensation){
        m_tauOut.data[i] = torque[i] + g_joint_torque[i];
      } else {
        m_tauOut.data[i] = torque[i];
      }
    }
    if (DEBUGP) {
      std::cerr << "[" <<  m_profile.instance_name << "]" << "filtered torque: ";
      for (int i = 0; i < num_joints; i++) {
        std::cerr << " " << torque[i];
      }
      std::cerr << std::endl;
    }
    m_tauOut.tm = tm;
    m_tauOutOut.write();
  } else {
    if (DEBUGP) {
      std::cerr << "[" <<  m_profile.instance_name << "]" << "input is not correct" << std::endl;
      std::cerr << "[" <<  m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
      std::cerr << "[" <<  m_profile.instance_name << "]" << " tauIn: " << m_tauIn.data.length() << std::endl;
      std::cerr << std::endl;
    }
  }
  
  return RTC::RTC_OK;
}
Ejemplo n.º 16
0
void GameState::cheat(Vehicle* ve, VehicleAI* veai, float id, bool isAi)
{
	//时间实在不足,多次优化车性能未果,只能让电脑不大好看的作弊方式前进


	//NxVec3 dir = NxVec3(veai->m_cDirect.x, 0, veai->m_cDirect.y);
	//ve->getActor()->addForce(dir*50, NX_IMPULSE);

	//Ogre::Vector3 dirOgre = ve->getCarNode()->getOrientation().zAxis();
	//dirOgre.x = dir.x;
	//dirOgre.z = dir.z;
	//mTempSceneNode->setOrientation(Ogre::Quaternion::IDENTITY);
	//mTempSceneNode->setDirection(dirOgre);
	//Ogre::Quaternion quat = mTempSceneNode->getOrientation();
	//NxQuat nxquat = UtilityFunc::OgreQuat_To_NxQuat(quat);
	//ve->getActor()->setGlobalOrientationQuat(nxquat);

	int lasti = veai->getNavigationCurrent();
	lasti = lasti-1 > 0 ? lasti-1 : 0;

	Ogre::Vector3 oriVec3(veai->getNavigationPoint(lasti));
	oriVec3.y += 2;

	ve->setOriginalPos(oriVec3);

	mTempSceneNode->setOrientation(Ogre::Quaternion::IDENTITY);

	Ogre::Vector3 dir(veai->m_cDirect.x, 0, veai->m_cDirect.y);
	mTempSceneNode->setDirection(-dir);

	ve->setOriginalQuat(mTempSceneNode->getOrientation());

	float speed = ve->getSpeed();

	if (speed >= 5.0f)
	{
		mSceneMgr->getParticleSystem("dust1_" + Ogre::StringConverter::toString(id))->setEmitting(true);
		mSceneMgr->getParticleSystem("dust2_"+ Ogre::StringConverter::toString(id))->setEmitting(true);
	}
	else
	{
		mSceneMgr->getParticleSystem("dust1_"+ Ogre::StringConverter::toString(id))->setEmitting(false);
		mSceneMgr->getParticleSystem("dust2_"+ Ogre::StringConverter::toString(id))->setEmitting(false);
	}

	NxWheelContactData d;
	for (int i=0;i<4;i++)
	{
		ve->mWheels[i].mWheel->getContact(d);
		if (d.otherShapeMaterialIndex == 18)
			ve->resetCarPos();
	}

	if (!isAi) return;
	if (mIsPause) return;
	if (win[id]) return;

	if (cur[id] < veai->getNavigationCurrent())
	{
		times[id] = 0;
		cur[id] = veai->getNavigationCurrent();
	}

	//if (speed < 1.0 && times[id] > timeReach[id]/2.0f)
	//{
	//	times[id] = 0;
	//	//ve->resetCarPos();
	//}

	NxVec3 force = UtilityFunc::OgreVec3_To_NxVec3(
		ve->getCarNode()->getOrientation().zAxis());

	//if (times[id] > timeReach[id])
	//{
	//	times[id] = 0;
	//	timeReach[id] = (float)(rand()%10 + 5) + rand()%10/10.0f;
	//	//ve->resetCarPos();
	//}
	//else
	//{
	//	times[id] += UtilityFunc::getElapseLastFrame();
	//}

	if (speed < 1.0f)
	{
		times[id] += UtilityFunc::getElapseLastFrame();
	}

	if (speed < 1.0f && times[id] > 3.0f)
	{
		ve->resetCarPos();
		times[id] = 0.0f;
	}

	NxVec3 torque(0, 200, 0);
	if (veai->getVehicleDirection() == VehicleAI::LEFT)
	{
		//ve->turnLeft(true);
		//ve->turnRight(false);
		if (speed < 1.0)
			ve->getActor()->addTorque(torque*4, NX_IMPULSE);
		else
			ve->getActor()->addTorque(torque, NX_IMPULSE);

		ve->getActor()->addForce(force*200, NX_IMPULSE);
	}
	else if (veai->getVehicleDirection() == VehicleAI::RIGHT)
	{
		//ve->turnLeft(false);
		//ve->turnRight(true);
		if (speed < 1.0)
			ve->getActor()->addTorque(-torque*3, NX_IMPULSE);
		else
			ve->getActor()->addTorque(-torque, NX_IMPULSE);
		ve->getActor()->addForce(force*200, NX_IMPULSE);
	}
	else
	{
		//NxVec3 force = NxVec3(veai->m_cDirect.x, 0, veai->m_cDirect.y);
		//car->getActor()->addForce(force*50, NX_IMPULSE);
		ve->getActor()->addForce(force*400, NX_IMPULSE);
		//ve->turnLeft(false);
		//ve->turnRight(false);
	}
	float sp = ve->getSpeed();
}
Ejemplo n.º 17
0
void Thruster::rollRight()
{
    torque(false);
}
Ejemplo n.º 18
0
void HoverVehicle::updateForces(F32 /*dt*/)
{
    PROFILE_SCOPE( HoverVehicle_UpdateForces );

    Point3F gravForce(0, 0, sHoverVehicleGravity * mRigid.mass * mGravityMod);

    MatrixF currTransform;
    mRigid.getTransform(&currTransform);
    mRigid.atRest = false;

    mThrustLevel = (mForwardThrust * mDataBlock->mainThrustForce    +
                    mReverseThrust * mDataBlock->reverseThrustForce +
                    mLeftThrust    * mDataBlock->strafeThrustForce  +
                    mRightThrust   * mDataBlock->strafeThrustForce);

    Point3F thrustForce = ((Point3F( 0,  1, 0) * (mForwardThrust * mDataBlock->mainThrustForce))    +
                           (Point3F( 0, -1, 0) * (mReverseThrust * mDataBlock->reverseThrustForce)) +
                           (Point3F(-1,  0, 0) * (mLeftThrust    * mDataBlock->strafeThrustForce))  +
                           (Point3F( 1,  0, 0) * (mRightThrust   * mDataBlock->strafeThrustForce)));
    currTransform.mulV(thrustForce);
    if (mJetting)
        thrustForce *= mDataBlock->turboFactor;

    Point3F torque(0, 0, 0);
    Point3F force(0, 0, 0);

    Point3F vel = mRigid.linVelocity;
    F32 baseStabLen = getBaseStabilizerLength();
    Point3F stabExtend(0, 0, -baseStabLen);
    currTransform.mulV(stabExtend);

    StabPoint stabPoints[2];
    stabPoints[0].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5,
                                    mObjBox.maxExtents.y,
                                    (mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5);
    stabPoints[1].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5,
                                    mObjBox.minExtents.y,
                                    (mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5);
    U32 j, i;
    for (i = 0; i < 2; i++) {
        currTransform.mulP(stabPoints[i].osPoint, &stabPoints[i].wsPoint);
        stabPoints[i].wsExtension = stabExtend;
        stabPoints[i].extension   = baseStabLen;
        stabPoints[i].wsVelocity  = mRigid.linVelocity;
    }

    RayInfo rinfo;

    mFloating = true;
    bool reallyFloating = true;
    F32 compression[2] = { 0.0f, 0.0f };
    F32  normalMod[2]  = { 0.0f, 0.0f };
    bool normalSet[2]  = { false, false };
    Point3F normal[2];

    for (j = 0; j < 2; j++) {
        if (getContainer()->castRay(stabPoints[j].wsPoint, stabPoints[j].wsPoint + stabPoints[j].wsExtension * 2.0,
                                    TerrainObjectType |
                                    WaterObjectType, &rinfo))
        {
            reallyFloating = false;

            if (rinfo.t <= 0.5) {
                // Ok, stab is in contact with the ground, let's calc the forces...
                compression[j] = (1.0 - (rinfo.t * 2.0)) * baseStabLen;
            }
            normalSet[j] = true;
            normalMod[j] = rinfo.t < 0.5 ? 1.0 : (1.0 - ((rinfo.t - 0.5) * 2.0));

            normal[j] = rinfo.normal;
        }

        if ( pointInWater( stabPoints[j].wsPoint ) )
            compression[j] = baseStabLen;
    }

    for (j = 0; j < 2; j++) {
        if (compression[j] != 0.0) {
            mFloating = false;

            // Spring force and damping
            Point3F springForce = -stabPoints[j].wsExtension;
            springForce.normalize();
            springForce *= compression[j] * mDataBlock->stabSpringConstant;

            Point3F springDamping = -stabPoints[j].wsExtension;
            springDamping.normalize();
            springDamping *= -getMin(mDot(springDamping, stabPoints[j].wsVelocity), 0.7f) * mDataBlock->stabDampingConstant;

            force += springForce + springDamping;
        }
    }

    // Gravity
    if (reallyFloating == false)
        force += gravForce;
    else
        force += gravForce * mDataBlock->floatingGravMag;

    // Braking
    F32 vellen = mRigid.linVelocity.len();
    if (mThrottle == 0.0f &&
            mLeftThrust == 0.0f &&
            mRightThrust == 0.0f &&
            vellen != 0.0f &&
            vellen < mDataBlock->brakingActivationSpeed)
    {
        Point3F dir = mRigid.linVelocity;
        dir.normalize();
        dir.neg();
        force += dir *  mDataBlock->brakingForce;
    }

    // Gyro Drag
    torque = -mRigid.angMomentum * mDataBlock->gyroDrag;

    // Move to proper normal
    Point3F sn, r;
    currTransform.getColumn(2, &sn);
    if (normalSet[0] || normalSet[1]) {
        if (normalSet[0] && normalSet[1]) {
            F32 dot = mDot(normal[0], normal[1]);
            if (dot > 0.999) {
                // Just pick the first normal.  They're too close to call
                if ((sn - normal[0]).lenSquared() > 0.00001) {
                    mCross(sn, normal[0], &r);
                    torque += r * mDataBlock->normalForce * normalMod[0];
                }
            } else {
                Point3F rotAxis;
                mCross(normal[0], normal[1], &rotAxis);
                rotAxis.normalize();

                F32 angle = mAcos(dot) * (normalMod[0] / (normalMod[0] + normalMod[1]));
                AngAxisF aa(rotAxis, angle);
                QuatF q(aa);
                MatrixF tempMat(true);
                q.setMatrix(&tempMat);
                Point3F newNormal;
                tempMat.mulV(normal[1], &newNormal);

                if ((sn - newNormal).lenSquared() > 0.00001) {
                    mCross(sn, newNormal, &r);
                    torque += r * (mDataBlock->normalForce * ((normalMod[0] + normalMod[1]) * 0.5));
                }
            }
        } else {
            Point3F useNormal;
            F32     useMod;
            if (normalSet[0]) {
                useNormal = normal[0];
                useMod    = normalMod[0];
            } else {
                useNormal = normal[1];
                useMod    = normalMod[1];
            }

            if ((sn - useNormal).lenSquared() > 0.00001) {
                mCross(sn, useNormal, &r);
                torque += r * mDataBlock->normalForce * useMod;
            }
        }
    } else {
        if ((sn - Point3F(0, 0, 1)).lenSquared() > 0.00001) {
            mCross(sn, Point3F(0, 0, 1), &r);
            torque += r * mDataBlock->restorativeForce;
        }
    }

    Point3F sn2;
    currTransform.getColumn(0, &sn);
    currTransform.getColumn(1, &sn2);
    mCross(sn, sn2, &r);
    r.normalize();
    torque -= r * (mSteering.x * mDataBlock->steeringForce);

    currTransform.getColumn(0, &sn);
    currTransform.getColumn(2, &sn2);
    mCross(sn, sn2, &r);
    r.normalize();
    torque -= r * (mSteering.x * mDataBlock->rollForce);

    currTransform.getColumn(1, &sn);
    currTransform.getColumn(2, &sn2);
    mCross(sn, sn2, &r);
    r.normalize();
    torque -= r * (mSteering.y * mDataBlock->pitchForce);

    // Apply drag
    Point3F vDrag = mRigid.linVelocity;
    if (!mFloating) {
        vDrag.convolve(Point3F(1, 1, mDataBlock->vertFactor));
    } else {
        vDrag.convolve(Point3F(0.25, 0.25, mDataBlock->vertFactor));
    }
    force -= vDrag * mDataBlock->dragForce;

    force += mFloating ? thrustForce * mDataBlock->floatingThrustFactor : thrustForce;

    // Add in physical zone force
    force += mAppliedForce;

    // Container buoyancy & drag
    force  += Point3F(0, 0,-mBuoyancy * sHoverVehicleGravity * mRigid.mass * mGravityMod);
    force  -= mRigid.linVelocity * mDrag;
    torque -= mRigid.angMomentum * mDrag;

    mRigid.force  = force;
    mRigid.torque = torque;
}
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{

	// get the simulation time
//	dFloat invTimestep = 1.0f / timestep ;

	// get the vehicle global matrix, and use it in several calculations
	dMatrix bodyMatrix;  
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyForce;
	dVector bodyOmega;
	dVector bodyVelocity;


	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

//static int xxx;
//dTrace (("frame %d veloc(%f %f %f)\n", xxx, bodyVelocity[0], bodyVelocity[1], bodyVelocity[2]));
//xxx ++;
//if (xxx >= 210) {
//xxx *=1;
//bodyVelocity.m_x = 0;
//bodyVelocity.m_z = 10;
//NewtonBodySetVelocity (m_body0, &bodyVelocity[0]);
//}

//	dVector normalForces (0.0f, 0.0f, 0.0f, 0.0f);
	// all tire is on air check
	m_vehicleOnAir = 0;
//	int constraintIndex = 0;
	for (int i = 0; i < m_tiresCount; i ++) {

//		dTrace (("tire: %d ", i));

		Tire& tire = m_tires[i];
		tire.m_tireIsOnAir = 1;
//		tire.m_tireIsConstrained = 0;	
		tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);

		// calculate all suspension matrices in global space and tire collision
		dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);

		// calculate the tire collision
		CalculateTireCollision (tire, suspensionMatrix, threadIndex);

		// calculate the linear velocity of the tire at the ground contact
		tire.m_tireAxelPositGlobal = chassisMatrix.TransformVector (tire.m_harpointInJointSpace - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVelocGlobal = bodyVelocity + bodyOmega * (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit); 
		tire.m_lateralPinGlobal = chassisMatrix.RotateVector (tire.m_localAxisInJointSpace);
		tire.m_longitudinalPinGlobal = chassisMatrix.m_up * tire.m_lateralPinGlobal;

		if (tire.m_posit < tire.m_suspensionLenght )  {

			tire.m_tireIsOnAir = 0;
			tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			if (tire.m_HitBody){
				dMatrix matrix;
				dVector com;
				dVector omega;

				NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
				NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
				NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
				NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
				tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
			} 


			// calculate the relative velocity
			dVector tireHubVeloc (tire.m_tireAxelVelocGlobal - tire.m_hitBodyPointVelocity);
			dFloat suspensionSpeed = - (tireHubVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			dFloat distance = tire.m_suspensionLenght - tire.m_posit;
			_ASSERTE (distance <= tire.m_suspensionLenght);
			tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
			if ( tire.m_tireLoad < 0.0f ) {
				// since the tire is not a body with real mass it can only push the chassis.
				tire.m_tireLoad = 0.0f;
			} 

			//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
			tire.m_tireLoad *= (m_mass * 0.5f);

//			dTrace (("(load = %f) ", tire.m_tireLoad));


			//tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);

			// convert the tire load force magnitude to a torque and force.
			// accumulate the force doe to the suspension spring and damper
			tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);


			// calculate relative velocity at the tire center
			//dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity); 

			// axle linear speed
			//axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;
			dFloat axelLinearSpeed = tireHubVeloc % chassisMatrix.m_front;

			// calculate tire rotation velocity at the tire radio
			//dVector tireAngularVelocity (tire.m_lateralPinGlobal.Scale (tire.m_angularVelocity));
			//dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPositGlobal);
			//dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);	


			// calculate slip ratio and max longitudinal force
			//dFloat tireRotationSpeed = -(tireRotationalVelocityAtContact % tire.m_longitudinalPinGlobal);
			//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;

			//dTrace (("(slipRatio = %f) ", slipRatioCoef));

			// calculate the formal longitudinal force the tire apply to the chassis
			//dFloat longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;

			dFloat longitudinalForceMag = CalculateLongitudinalForce (i, axelLinearSpeed, tire.m_tireLoad * tire.m_groundFriction);

//			dTrace (("(longForce = %f) ", longitudinalForceMag));

#if 0

			// now calculate relative velocity a velocity at contact point
			//dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact); 
			//dVector tireContactAbsoluteVelocity (tireHubVeloc + tireRotationalVelocityAtContact); 

			// calculate the side slip as the angle between the tire lateral speed and longitudinal speed 
			//dFloat lateralSpeed = tireContactRelativeVelocity % tire.m_lateralPin;
			dFloat lateralSpeed = tireHubVeloc % tire.m_lateralPinGlobal;

			dFloat sideSlipCoef = dAtan2 (dAbs (lateralSpeed), dAbs (axelLinearSpeed));
			dFloat lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;

			// Apply brake, need some little fix here.
			// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
			if ( tire.m_breakForce > 1.0e-3f ) {
				_ASSERTE (0);
/*
				// row constrained force is save for later determine the dynamic state of this tire 
  				tire.m_isBrakingForceIndex = constraintIndex;
				constraintIndex ++;

				frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
				if (tire.m_breakForce > frictionCircleMag) {
					tire.m_breakForce = frictionCircleMag;
				}

				//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x  );
				NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
				NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
				NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);

				// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
				tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
				if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
  					lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
				}
*/
			} 


			//project the longitudinal and lateral forces over the circle of friction for this tire; 
			dFloat frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;

			dFloat tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
			if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
				dFloat invMag2;
				invMag2 = frictionCircleMag / dSqrt (tireForceMag);
				longitudinalForceMag *= invMag2;
				lateralFrictionForceMag *= invMag2;
			}


			// submit this constraint for calculation of side slip forces
			lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
			tire.m_lateralForceIndex = constraintIndex;
			constraintIndex ++;
			NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPositGlobal[0], &tire.m_tireAxelPositGlobal[0], &tire.m_lateralPinGlobal[0]);
			NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
			NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
#endif

			// accumulate the longitudinal force
			dVector tireForce (tire.m_longitudinalPinGlobal.Scale (longitudinalForceMag));
			tire.m_tireForceAcc += tireForce;

			// now we apply the combined tire force generated by this tire, to the car chassis
			dVector r (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);

			// add the toque the tire asserts on the car body (principle of action reaction)
			dVector torque (r * tire.m_tireForceAcc - tire.m_lateralPinGlobal.Scale (tire.m_torque));
			NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
			NewtonBodyAddTorque( m_body0, &torque[0] );
/*
			// calculate the net torque on the tire
			dFloat tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPinGlobal);
			if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
				// the tire reaction force cannot be larger than the applied engine torque 
				// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
				tire.m_tireIsConstrained = 1;
				tireTorqueMag = tire.m_torque;
			}

			tire.m_torque -= tireTorqueMag;
*/
//			normalForces += tire.m_tireForceAcc;

		} else {

			// there is a next torque on the tire
			tire.m_torque -= tire.m_angularVelocity * tire.m_Ixx * DG_TIRE_VISCUOS_DAMP;
			tire.m_angularVelocity += tire.m_torque * tire.m_IxxInv * timestep;
			if (m_tires[i].m_breakForce > dFloat (0.1f)) {
				tire.m_angularVelocity = 0.0f;
			}
		}

//		dTrace (("(tireTorque = %f) ", tire.m_torque));

		// spin the tire by the angular velocity
		tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);

		// reset the tire torque
		tire.m_torque = 0.0f;
		tire.m_breakForce = 0.0f;  

//		dTrace (("\n"));

	}


	// add a row to simulate the engine rolling resistance
//	float bodyWeight = dAbs (normalForces % chassisMatrix.m_up) * m_rollingResistance;
//	if (bodyWeight > (1.0e-3f) * m_mass) {
//		NewtonUserJointAddLinearRow (m_joint, &chassisMatrix.m_posit[0], &chassisMatrix.m_posit[0], &chassisMatrix.m_front[0]);
//		NewtonUserJointSetRowMaximumFriction( m_joint,  bodyWeight);
//		NewtonUserJointSetRowMinimumFriction( m_joint, -bodyWeight);
//	}
}
int main( int argc , char** argv ) {

  int L_flag=0,i,j,k,l;

  if ( argc > 1 and !strcmp( "-nt" , argv[1] ) ){
    nthreads = atoi( argv[2] ) ;
    omp_set_num_threads( nthreads ) ;
    printf("\nNumber of threads set to %d!\n" , nthreads ) ;
  }
  else {
    nthreads = 1 ;
    omp_set_num_threads( nthreads ) ;
    printf("\nNumber of threads set to %d!\n" , nthreads ) ;
   }

  read_input() ;

   if(argc == 5 ) {
     flux_sp =  atoi(argv[4]);
     cout<<"new flus sp "<<flux_sp<<endl;
   }
  
  initialize() ;

  write_gro( ) ;
  write_quaternions( ) ;
   write_film_gro();
  // Save chi for pre-equilibration steps //
  double tmp_ang,chi_bkp = chiAB ;

  FILE *otp ,*otpL;
  otp = fopen( "data.dat" , "w" ) ;
  otpL = fopen("box_L.dat","w");

  printf("Entering main loop!\n") ; fflush( stdout ) ;
  for ( step = 0 ; step < nsteps ; step++ )  {

    if ( step < pre_equil_steps )
      chiAB = 0.0 ;
    else
      chiAB = chi_bkp ;
   

    forces() ;
  

    if(sigma>0){
       torque();
    }

   // if(step >0 || rst_para == 1)
   	update_positions() ;
   // else
    //	update_positions_init() ;
    
    if(sigma>0){
    update_euler();
    }
   
    if( ( flux_para == 1) and (step % flux_sp == 0)){


	 flux();

    }
    else if((flux_para == 2 ) and (step % flux_sp == 0) and (max_nC> nC ) and (nsol>Nhc)){

         flux();	
    }
    else{

    }


	 

    if ( stress_freq > 0 && step % stress_freq == 0 ) {
      calc_stress() ;

      for ( j=0 ; j<Dim ; j++ ){ 
        for ( k=0 ; k<Dim ; k++ ) {
          sts_buf[buff_ind][j][k]= Rg3*Ptens[j][k];//( j<Dim ? Stress_bonds[j][k]:0.0) ;
           sts_buf_pp[buff_ind][j][k] = Rg3*Stress_PP[j][k];
      	   sts_buf_ng[buff_ind][j][k] = Rg3*Stress_Ng[j][k];
            
	    if( ((L_fren-L_aver) < L_flag)  and (optm_L >0) and (j ==k))
	        aver_Ptens[j][j] += Rg3*Ptens[j][j];


	/*for ( k=0 ; k<nP;  k++ ){
		//euler_adot[k][j] = euler_q[k][j];	
		sts_buf[buff_ind][j][k+Dim] = euler_q[k][j];
	}*/
	}
      }
      if(((L_fren-L_aver) < L_flag) and   (optm_L >0))
      aver_Ptens[0][1] +=1;
 
      buff_ind++ ;
    }

   if(optm_L>0  ){
    	if( step > pre_equil_steps )
       		L_flag +=1 ;

    	if(L_flag == L_fren){
       		adj_L();
		L_flag = 0 ;
               
		
		
	        fprintf( otpL , "%d %lf %lf %lf \n" ,step ,L[0],L[1],L[2]);fflush( otpL ) ;
	}		
   }//optm_L

    if(step % sample_freq == 0){
	write_np();
        //write_film_gro();
        char nm[20];

	if(nsol > 0){
	   sprintf( nm , "./frame/rhosol.frame%d.dat" , step ) ;
	   write_grid_data( nm , rhosol ) ;
	}
	if(nD > 0){
	   sprintf( nm , "./frame/rhoda.frame%d.dat" , step ) ;
	    write_grid_data( nm , rhoda ) ;
	   sprintf( nm , "./frame/rhodb.frame%d.dat" , step ) ;
	               write_grid_data( nm , rhodb ) ;
	}
    }  
    
    if ( step > sample_wait && step % sample_freq == 0 ) {
     /* fftw_fwd( rho[0] , ktmp ) ;
      for ( i=0 ; i<M ; i++ ) {
        avg_sk[0][i] += ktmp[i] * conj(ktmp[i]) ;
      }

      if ( nP > 0 ) {

        fftw_fwd( rho[2] , ktmp ) ;
        for ( i=0 ; i<M ; i++ ) {
          avg_sk[2][i] += ktmp[i] * conj( ktmp[i] ) ;
        }
      }*/
     /* for ( i=0 ; i<M ; i++ ) {
        avg_rho[0][i] += rho[0][i];
	avg_rho[1][i] += rho[1][i];
        //avg_rho[3][i] += rho[3][i];
       
      }*/
	


      num_averages += 1.0 ;
    }


    if ( step % print_freq == 0 || step == nsteps-1 ) {
      printf("step %d of %d  Ubond: %lf\n" , step , nsteps , Ubond ) ;
      fflush( stdout ) ;
      write_gro() ;
      write_rst_gro();
      write_quaternions();


      if ( stress_freq > 0 )
        write_stress() ;


      write_grid_data( "rhoda.dat" , rhoda ) ;
      write_grid_data( "rhodb.dat" , rhodb ) ;
      
      if ( nA > 0.0 )
        write_grid_data( "rhoha.dat" , rhoha ) ;

      if ( nB > 0.0 )
        write_grid_data( "rhohb.dat" , rhohb ) ;
      if(  nC > 0.0 )
         write_grid_data( "rhohc.dat" , rhohb ) ;

      if ( nP > 0.0 ) 
        write_grid_data( "rhop.dat" , rhop ) ;

      if ( step > sample_wait ) {
       /* for ( i=0 ; i<M ; i++ ) 
          ktmp2[i] = avg_sk[0][i] / num_averages ;
        write_kspace_data( "avg_sk_A.dat" , ktmp2 ) ;
        
        if ( nP > 0 ) {
          for ( i=0 ; i<M ; i++ )
            ktmp2[i] = avg_sk[2][i] / num_averages ;
          write_kspace_data( "avg_sk_np.dat" , ktmp2 ) ;
        }*/
        /*	
        for ( i=0 ; i<M ; i++ )
	     tmp[i] = avg_rho[0][i]/ num_averages ;
         write_grid_data("avg_typeA.dat",tmp);	

	for ( i=0 ; i<M ; i++ )
	     tmp[i] = avg_rho[1][i]/ num_averages ;
         write_grid_data("avg_typeB.dat",tmp);	

        for ( i=0 ; i<M ; i++ )
	     tmp[i] = avg_rho[3][i]/ num_averages ;
         write_grid_data("avg_typeC.dat",tmp);	
	*/

      }

      calc_Unb() ;

      fprintf( otp , "%d %lf %lf %lf %lf %lf %lf %lf\n" , step , Ubond , U_chi_gg, U_kappa_gg,U_chi_pg,U_kappa_pg,U_kappa_pp , Utt) ;
      fflush( otp ) ;

    }// if step % print_Freq == 0

  }

  fclose( otp ) ;

  return 0 ;

}
Expression<Vector>::Ptr Torque_Wrench::derivativeExpression(int i) {
    return torque(argument->derivativeExpression(i));
}
Ejemplo n.º 22
0
//---------------------------------------------------------
//---------------------------------------------------------
void CBounceBomb::SettleThink()
{
	SetNextThink( gpGlobals->curtime + 0.05 );
	StudioFrameAdvance();

	if( GetParent() )
	{
		// A scanner or something is carrying me. Just keep checking back.
		return;
	}

	// Not being carried.
	if( !VPhysicsGetObject() )
	{
		// Probably was just dropped. Get physics going.
		CreateVPhysics();

		if( !VPhysicsGetObject() )
		{
			Msg("**** Can't create vphysics for combine_mine!\n" );
			UTIL_Remove( this );
			return;
		}

		VPhysicsGetObject()->Wake();
		return;
	}

	if( !m_bDisarmed )
	{
		if( VPhysicsGetObject()->IsAsleep() && !(VPhysicsGetObject()->GetGameFlags() & FVPHYSICS_PLAYER_HELD) )
		{
			// If i'm not resting on the world, jump randomly.
			trace_t tr;
			UTIL_TraceLine( GetAbsOrigin(), GetAbsOrigin() - Vector( 0, 0, 1024 ), MASK_SHOT|CONTENTS_GRATE, this, COLLISION_GROUP_NONE, &tr );

			bool bHop = false;
			if( tr.m_pEnt )
			{
				IPhysicsObject *pPhysics = tr.m_pEnt->VPhysicsGetObject();

				if( pPhysics && pPhysics->GetMass() <= 1000 )
				{
					// Light physics objects can be moved out from under the mine.
					bHop = true;
				}
				else if( tr.m_pEnt->m_takedamage != DAMAGE_NO )
				{
					// Things that can be harmed can likely be broken.
					bHop = true;
				}

				if( bHop )
				{
					Vector vecForce;
					vecForce.x = random->RandomFloat( -1000, 1000 );
					vecForce.y = random->RandomFloat( -1000, 1000 );
					vecForce.z = 2500;

					AngularImpulse torque( 160, 0, 160 );

					Flip( vecForce, torque );
					return;
				}

				// Check for upside-down
				Vector vecUp;
				GetVectors( NULL, NULL, &vecUp );
				if( vecUp.z <= 0.8 )
				{
					// Landed upside down. Right self
					Vector vecForce( 0, 0, 2500 );
					Flip( vecForce, AngularImpulse( 60, 0, 0 ) );
					return;
				}
			}

			// Check to make sure I'm not in a forbidden location
			if( !IsValidLocation() )
			{
				return;
			}

			// Lock to what I'm resting on
			constraint_ballsocketparams_t ballsocket;
			ballsocket.Defaults();
			ballsocket.constraint.Defaults();
			ballsocket.constraint.forceLimit = lbs2kg(1000);
			ballsocket.constraint.torqueLimit = lbs2kg(1000);
			ballsocket.InitWithCurrentObjectState( g_PhysWorldObject, VPhysicsGetObject(), GetAbsOrigin() );
			m_pConstraint = physenv->CreateBallsocketConstraint( g_PhysWorldObject, VPhysicsGetObject(), NULL, ballsocket );
			CloseHooks();

			SetMineState( MINE_STATE_ARMED );
		}
	}
}
// Move Matsya
void ThrusterPlugin::applyThrusterForce()
{
    math::Vector3 force(0.0,0.0,0.0);
    math::Vector3 torque(0.0,0.0,0.0);
    math::Quaternion poseRot = _baseLink->GetWorldPose().rot;
    for(int i=0; i<NO_OF_THRUSTERS; ++i)
    {
        // if(_pwm.values[i] > 1023)
        //     _pwm.values[i] = 1023;
        // else if(_pwm.values[i] < 0)
        //     _pwm.values[i] = 0;

        switch(i)
        {
            case THRUSTER_SURGE_LEFT :
                force.x += (_pwm.values[THRUSTER_SURGE_LEFT]
                        -THRUSTER_STOP_PWM)/FORCE_PWM_FACTOR;
                torque.z += -(_pwm.values[THRUSTER_SURGE_LEFT]
                        -THRUSTER_STOP_PWM)/TORQUE_PWM_FACTOR;
                break;
            case THRUSTER_SURGE_RIGHT :
                force.x += (_pwm.values[THRUSTER_SURGE_RIGHT]
                        -THRUSTER_STOP_PWM)/FORCE_PWM_FACTOR;
                torque.z += (_pwm.values[THRUSTER_SURGE_RIGHT]
                        -THRUSTER_STOP_PWM)/TORQUE_PWM_FACTOR;
                break;
            case THRUSTER_SWAY_BACK :
                force.y += -(_pwm.values[THRUSTER_SWAY_BACK]
                        -THRUSTER_STOP_PWM)/FORCE_PWM_FACTOR;
                torque.z += -(_pwm.values[THRUSTER_SWAY_BACK]
                        -THRUSTER_STOP_PWM)/TORQUE_PWM_FACTOR;
                break;
            case THRUSTER_SWAY_FRONT :
                force.y += (_pwm.values[THRUSTER_SWAY_FRONT]
                        -THRUSTER_STOP_PWM)/FORCE_PWM_FACTOR;
                torque.z += -(_pwm.values[THRUSTER_SWAY_FRONT]
                        -THRUSTER_STOP_PWM)/TORQUE_PWM_FACTOR;
                break;
            case THRUSTER_DEPTH_BACK :
                force.z += -(_pwm.values[THRUSTER_DEPTH_BACK]
                        -THRUSTER_STOP_PWM)/FORCE_PWM_FACTOR;
                torque.y += -(_pwm.values[THRUSTER_DEPTH_BACK]
                        -THRUSTER_STOP_PWM)/TORQUE_PWM_FACTOR;
                break;
            case THRUSTER_DEPTH_FRONT :
                force.z += -(_pwm.values[THRUSTER_DEPTH_FRONT]
                        -THRUSTER_STOP_PWM)/FORCE_PWM_FACTOR;
                torque.y += (_pwm.values[THRUSTER_DEPTH_FRONT]
                        -THRUSTER_STOP_PWM)/TORQUE_PWM_FACTOR;
                break;
            default :
                break;
        }
    }
    torque.x = 0;
    force = poseRot.RotateVector(force);
    torque = poseRot.RotateVector(torque);
    // gzdbg<<force<<'\t'<<torque<<'\t';
    _baseLink->AddRelativeTorque(torque);
    _baseLink->AddRelativeForce(force);
    // gzdbg<<_baseLink->GetWorldPose()<<'\n';
}
Ejemplo n.º 24
0
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
	dVector com; 
	dMatrix matrix; 
	dVector omega0;
	dVector veloc0;
	dVector omega1;
	dVector veloc1;
	dVector pointVeloc;

	const dFloat stiffness = 0.3f;
	const dFloat angularDamp = 0.95f;

	dFloat invTimeStep = 1.0f / timestep;
	NewtonWorld* const world = NewtonBodyGetWorld (body);
	NewtonWorldCriticalSectionLock (world, 0);

	// calculate the desired impulse
	NewtonBodyGetMatrix(body, &matrix[0][0]);
	NewtonBodyGetOmega (body, &omega0[0]);
	NewtonBodyGetVelocity (body, &veloc0[0]);

	NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);

	dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
	deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
	for (int i = 0; i < 3; i ++) {
		dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
		veloc[i] = deltaVeloc[i];
		NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
	}

	// damp angular velocity
	NewtonBodyGetOmega (body, &omega1[0]);
	NewtonBodyGetVelocity (body, &veloc1[0]);
	omega1 = omega1.Scale (angularDamp);

	// restore body velocity and angular velocity
	NewtonBodySetOmega (body, &omega0[0]);
	NewtonBodySetVelocity(body, &veloc0[0]);

	// convert the delta velocity change to a external force and torque
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);

	dVector angularMomentum (Ixx, Iyy, Izz);
	angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));

	dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
	dVector torque (angularMomentum.Scale(invTimeStep));

	NewtonBodyAddForce(body, &force[0]);
	NewtonBodyAddTorque(body, &torque[0]);

	// make sure the body is unfrozen, if it is picked
	NewtonBodySetSleepState (body, 0);

	NewtonWorldCriticalSectionUnlock (world);
}
Ejemplo n.º 25
0
void dComplentaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
{
	dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
	dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];

	int stateIndex = 0;
	dVector zero(dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f));
	for (int i = 0; i < bodyCount; i ++) {
		dBodyState* const state = bodyArray[i];
		stateVeloc[stateIndex].m_linear = state->m_veloc;
		stateVeloc[stateIndex].m_angular = state->m_omega;

		internalForces[stateIndex].m_linear = zero;
		internalForces[stateIndex].m_angular = zero;

		state->m_myIndex = stateIndex;
		stateIndex ++;
		dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
	}

	for (int i = 0; i < jointCount; i ++) {
		dJacobian y0;
		dJacobian y1;
		y0.m_linear = zero;
		y0.m_angular = zero;
		y1.m_linear = zero;
		y1.m_angular = zero;
		dBilateralJoint* const constraint = jointArray[i];
		int first = constraint->m_start;
		int count = constraint->m_count;
		for (int j = 0; j < count; j ++) { 
			dJacobianPair* const row = &jacobianArray[j + first];
			const dJacobianColum* const col = &jacobianColumnArray[j + first];
			dFloat val = col->m_force; 
			y0.m_linear += row->m_jacobian_IM0.m_linear.Scale(val);
			y0.m_angular += row->m_jacobian_IM0.m_angular.Scale(val);
			y1.m_linear += row->m_jacobian_IM1.m_linear.Scale(val);
			y1.m_angular += row->m_jacobian_IM1.m_angular.Scale(val);
		}
		int m0 = constraint->m_state0->m_myIndex;
		int m1 = constraint->m_state1->m_myIndex;
		internalForces[m0].m_linear += y0.m_linear;
		internalForces[m0].m_angular += y0.m_angular;
		internalForces[m1].m_linear += y1.m_linear;
		internalForces[m1].m_angular += y1.m_angular;
	}


	dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
	dFloat invStep = dFloat (0.25f);
	dFloat timestep = timestepSrc * invStep;
	dFloat invTimestep = invTimestepSrc * dFloat (4.0f);

	int maxPasses = 5;
	dFloat firstPassCoef = dFloat (0.0f);
	dFloat maxAccNorm = dFloat (1.0e-2f);

	for (int step = 0; step < 4; step ++) {
		dJointAccelerationDecriptor joindDesc;
		joindDesc.m_timeStep = timestep;
		joindDesc.m_invTimeStep = invTimestep;
		joindDesc.m_firstPassCoefFlag = firstPassCoef;

		for (int i = 0; i < jointCount; i ++) {
			dBilateralJoint* const constraint = jointArray[i];
			joindDesc.m_rowsCount = constraint->m_count;
			joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
			joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
			constraint->JointAccelerations (&joindDesc);
		}
		firstPassCoef = dFloat (1.0f);

		dFloat accNorm = dFloat (1.0e10f);
		for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
			accNorm = dFloat (0.0f);
			for (int i = 0; i < jointCount; i ++) {

				dBilateralJoint* const constraint = jointArray[i];
				int index = constraint->m_start;
				int rowsCount = constraint->m_count;
				int m0 = constraint->m_state0->m_myIndex;
				int m1 = constraint->m_state1->m_myIndex;

				dVector linearM0 (internalForces[m0].m_linear);
				dVector angularM0 (internalForces[m0].m_angular);
				dVector linearM1 (internalForces[m1].m_linear);
				dVector angularM1 (internalForces[m1].m_angular);

				dBodyState* const state0 = constraint->m_state0;
				dBodyState* const state1 = constraint->m_state1;
				const dMatrix& invInertia0 = state0->m_invInertia;
				const dMatrix& invInertia1 = state1->m_invInertia;
				dFloat invMass0 = state0->m_invMass;
				dFloat invMass1 = state1->m_invMass;

				for (int k = 0; k < rowsCount; k ++) {
					dJacobianPair* const row = &jacobianArray[index];
					dJacobianColum* const col = &jacobianColumnArray[index];

					dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
					dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
					dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
					dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
					dVector acc (JMinvIM0linear.CompProduct(linearM0) + JMinvIM0angular.CompProduct(angularM0) + JMinvIM1linear.CompProduct(linearM1) + JMinvIM1angular.CompProduct(angularM1));

					dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
					dFloat f = col->m_force + col->m_invDJMinvJt * a;

					dFloat lowerFrictionForce = col->m_jointLowFriction;
					dFloat upperFrictionForce = col->m_jointHighFriction;

					if (f > upperFrictionForce) {
						a = dFloat (0.0f);
						f = upperFrictionForce;
					} else if (f < lowerFrictionForce) {
						a = dFloat (0.0f);
						f = lowerFrictionForce;
					}

					accNorm = dMax (accNorm, dAbs (a));
					dFloat prevValue = f - col->m_force;
					col->m_force = f;

					linearM0 += row->m_jacobian_IM0.m_linear.Scale (prevValue);
					angularM0 += row->m_jacobian_IM0.m_angular.Scale (prevValue);
					linearM1 += row->m_jacobian_IM1.m_linear.Scale (prevValue);
					angularM1 += row->m_jacobian_IM1.m_angular.Scale (prevValue);
					index ++;
				}
				internalForces[m0].m_linear = linearM0;
				internalForces[m0].m_angular = angularM0;
				internalForces[m1].m_linear = linearM1;
				internalForces[m1].m_angular = angularM1;
			}
		}

		for (int i = 0; i < bodyCount; i ++) {
			dBodyState* const state = bodyArray[i];
			//int index = state->m_myIndex;
			dAssert (state->m_myIndex == i);
			dVector force (state->m_externalForce + internalForces[i].m_linear);
			dVector torque (state->m_externalTorque + internalForces[i].m_angular);
			state->IntegrateForce(timestep, force, torque);
		}
	}

	for (int i = 0; i < jointCount; i ++) {
		dBilateralJoint* const constraint = jointArray[i];
		int first = constraint->m_start;
		int count = constraint->m_count;
		for (int j = 0; j < count; j ++) { 
			const dJacobianColum* const col = &jacobianColumnArray[j + first];
			dFloat val = col->m_force; 
			constraint->m_jointFeebackForce[j] = val;
		}
	}

	for (int i = 0; i < jointCount; i ++) {
		dBilateralJoint* const constraint = jointArray[i];
		constraint->UpdateSolverForces (jacobianArray);
	}

	for (int i = 0; i < bodyCount; i ++) {
		dBodyState* const state = bodyArray[i];
		dAssert (state->m_myIndex == i);
		state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
	}
}
Ejemplo n.º 26
0
void updateHaptics(void)
{
    // initialize frequency counter
    frequencyCounter.reset();

    // main haptic simulation loop
    while(simulationRunning)
    {
        /////////////////////////////////////////////////////////////////////
        // READ HAPTIC DEVICE
        /////////////////////////////////////////////////////////////////////

        // read position
        cVector3d position;
        hapticDevice->getPosition(position);

        // read orientation
        cMatrix3d rotation;
        hapticDevice->getRotation(rotation);

        // read gripper position
        double gripperAngle;
        hapticDevice->getGripperAngleRad(gripperAngle);

        // read linear velocity
        cVector3d linearVelocity;
        hapticDevice->getLinearVelocity(linearVelocity);

        // read angular velocity
        cVector3d angularVelocity;
        hapticDevice->getAngularVelocity(angularVelocity);

        // read gripper angular velocity
        double gripperAngularVelocity;
        hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);

        // read userswitch status (button 0)
        bool button0, button1, button2, button3;
        button0 = false;
        button1 = false;
        button2 = false;
        button3 = false;

        hapticDevice->getUserSwitch(0, button0);
        hapticDevice->getUserSwitch(1, button1);
        hapticDevice->getUserSwitch(2, button2);
        hapticDevice->getUserSwitch(3, button3);

        /////////////////////////////////////////////////////////////////////
        // UPDATE 3D MODELS
        /////////////////////////////////////////////////////////////////////

        // update arrow
        velocity->m_pointA = position;
        velocity->m_pointB = cAdd(position, linearVelocity);

        // update position and orientation of cursor
        cursor->setLocalPos(position);
        cursor->setLocalRot(rotation);

        // adjust the  color of the cursor according to the status of
        // the user switch (ON = TRUE / OFF = FALSE)
        if (button0)
        {
            cursor->m_material->setGreenMediumAquamarine();
        }
        else if (button1)
        {
            cursor->m_material->setYellowGold();
        }
        else if (button2)
        {
            cursor->m_material->setOrangeCoral();
        }
        else if (button3)
        {
            cursor->m_material->setPurpleLavender();
        }
        else
        {
            cursor->m_material->setBlueRoyal();
        }

        // update global variable for graphic display update
        hapticDevicePosition = position;


        /////////////////////////////////////////////////////////////////////
        // COMPUTE AND SEND FORCE AND TORQUE
        /////////////////////////////////////////////////////////////////////

        cVector3d force (0,0,0);
        cVector3d torque (0,0,0);
        double gripperForce = 0.0;

        // apply force field
        if (useForceField)
        {
            // compute linear force
            double Kp = 20; // [N/m]
            cVector3d forceField = -Kp * position;
            force.add(forceField);

            // compute angular torque
            double Kr = 0.05; // [N/m.rad]
            cVector3d axis;
            double angle;
            rotation.toAngleAxis(angle, axis);
            torque = (-Kr * angle) * axis;
        }

        // apply damping term
        if (useDamping)
        {
            cHapticDeviceInfo info = hapticDevice->getSpecifications();

            // compute linear damping force
            double Kv = 1.0 * info.m_maxLinearDamping;
            cVector3d forceDamping = -Kv * linearVelocity;
            force.add(forceDamping);

            // compute angluar damping force
            double Kvr = 1.0 * info.m_maxAngularDamping;
            cVector3d torqueDamping = -Kvr * angularVelocity;
            torque.add(torqueDamping);

            // compute gripper angular damping force
            double Kvg = 1.0 * info.m_maxGripperAngularDamping;
            gripperForce = gripperForce - Kvg * gripperAngularVelocity;
        }

        // send computed force, torque and gripper force to haptic device
        hapticDevice->setForceAndTorqueAndGripperForce(force, torque, gripperForce);

        // update frequency counter
        frequencyCounter.signal(1);
    }

    // exit haptics thread
    simulationFinished = true;
}
Ejemplo n.º 27
0
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
	int constraintIndex;
	dFloat invTimestep;
	dMatrix bodyMatrix;  

	// get the simulation time
	invTimestep = 1.0f / timestep ;

	// get the vehicle global matrix, and use it in several calculations
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyOmega;
	dVector bodyVelocity;
	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

	// all tire is on air check
	m_vehicleOnAir = 0;
	constraintIndex = 0;
	for ( int i = 0; i < m_tiresCount; i ++ ) {

		Tire& tire = m_tires[i];
		tire.m_tireIsOnAir = 1;
		tire.m_tireIsConstrained = 0;	
		tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);

		// calculate all suspension matrices in global space and tire collision
		dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);

		// calculate the tire collision
		CalculateTireCollision (tire, suspensionMatrix, threadIndex);

		// calculate the linear velocity of the tire at the ground contact
		tire.m_tireAxelPosit = chassisMatrix.TransformVector( tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVeloc = bodyVelocity + bodyOmega * (tire.m_tireAxelPosit - chassisMatrix.m_posit); 
		tire.m_lateralPin = ( chassisMatrix.RotateVector ( tire.m_localAxis ) );
		tire.m_longitudinalPin = ( chassisMatrix.m_up * tire.m_lateralPin );

		if (tire.m_posit < tire.m_suspensionLenght )  {
			dFloat distance;
			dFloat sideSlipCoef;
			dFloat slipRatioCoef;
			dFloat tireForceMag;
			dFloat tireTorqueMag;
			dFloat suspensionSpeed;
			dFloat axelLinealSpeed;
			dFloat tireRotationSpeed;
			dFloat frictionCircleMag;
			dFloat longitudinalForceMag;
			dFloat lateralFrictionForceMag;


			tire.m_tireIsOnAir = 0;
			tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			if (tire.m_HitBody){
				dMatrix matrix;
				dVector com;
				dVector omega;

				NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
				NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
				NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
				NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
				tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
			} 

			// calculate the relative velocity
			dVector relVeloc (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
			suspensionSpeed = - (relVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			distance = tire.m_suspensionLenght - tire.m_posit;
			_ASSERTE (distance <= tire.m_suspensionLenght);
			tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
			if ( tire.m_tireLoad < 0.0f ) {
				// since the tire is not a body with real mass it can only push the chassis.
				tire.m_tireLoad = 0.0f;
			} 

			//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
			tire.m_tireLoad *= (m_mass * 0.5f);

			tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);

			// convert the tire load force magnitude to a torque and force.
			// accumulate the force doe to the suspension spring and damper
			tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);

			// calculate relative velocity at the tire center
			dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity); 

			// axle linear speed
			axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;

			// calculate tire rotation velocity at the tire radio
			dVector tireAngularVelocity (tire.m_lateralPin.Scale (tire.m_angularVelocity));
			dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
			dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);	

			longitudinalForceMag = 0.0f;
//			if (!tire.m_tireIsConstrained) {
				
				// calculate slip ratio and max longitudinal force
				tireRotationSpeed = tireRotationalVelocityAtContact % tire.m_longitudinalPin;
				slipRatioCoef = (dAbs (axelLinealSpeed) > 1.e-3f) ? ((-tireRotationSpeed - axelLinealSpeed) / dAbs (axelLinealSpeed)) : 0.0f;

				// calculate the formal longitudinal force the tire apply to the chassis
				longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
//			} 

		
			// now calculate relative velocity a velocity at contact point
			dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact); 

			// calculate the sideslip as the angle between the tire lateral speed and longitudila speed 
			sideSlipCoef = dAtan2 (dAbs (tireContactRelativeVelocity % tire.m_lateralPin), dAbs (axelLinealSpeed));
			lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;

			// Apply brake, need some little fix here.
			// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
			if ( tire.m_breakForce > 1.0e-3f ) {
				// row constrained force is save for later determine the dynamic state of this tire 
  				tire.m_isBrakingForceIndex = constraintIndex;
				constraintIndex ++;

				frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
				if (tire.m_breakForce > frictionCircleMag) {
					tire.m_breakForce = frictionCircleMag;
				}

				//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x  );
				NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
				NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
				NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);

				// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
				tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
				if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
  					lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
				}
			} 


			//project the longitudinal and lateral forces over the circle of friction for this tire; 
			frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
			tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
			if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
				dFloat invMag2;
				invMag2 = frictionCircleMag / dSqrt (tireForceMag);
				longitudinalForceMag *= invMag2;
				lateralFrictionForceMag *= invMag2;
			}

			// submit this constraint for calculation of side slip forces
			lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
			tire.m_lateralForceIndex = constraintIndex;
			constraintIndex ++;
			NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_lateralPin[0]);
			NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
			NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);

			// accumulate the longitudinal force
			dVector tireForce (tire.m_longitudinalPin.Scale (longitudinalForceMag));
			tire.m_tireForceAcc += tireForce;

			// now we apply the combined tire force generated by this tire, to the car chassis
			dVector torque ((tire.m_tireAxelPosit - chassisMatrix.m_posit) * tire.m_tireForceAcc);
			NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
			NewtonBodyAddTorque( m_body0, &torque[0] );


			// calculate the net torque on the tire
			tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPin);
			if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
				// the tire reaction force can no be larger than the applied engine torque 
				// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
				tire.m_tireIsConstrained = 1;
				tireTorqueMag = tire.m_torque;
			}

			tire.m_torque -= tireTorqueMag;
		} 	
	}
}
/*! One possible version of the GFO problem.

	Given a matrix of joint torques applied to the robot joints, this will check
	if there exists a set of legal contact forces that balance them. If so, it
	will compute the set of contact forces that adds up to the wrench of least
	magnitude on the object.

	For now, this output of this function is to set the computed contact forces
	on each contact as dynamic forces, and also to accumulate the resulting
	wrench on the object in its external wrench accumulator.

	Return codes: 0 is success, >0 means finger slip, no legal contact forces 
	exist; <0 means error in computation 
*/
int 
Grasp::computeQuasistaticForces(const Matrix &robotTau)
{
	//WARNING: for now, this ignores contacts on the palm. That might change in the future

	//for now, if the hand is touching anything other then the object, abort
	for (int c=0; c<hand->getNumChains(); c++) {
		if ( hand->getChain(c)->getNumContacts(NULL) !=
			hand->getChain(c)->getNumContacts(object) ) {
				DBGA("Hand contacts not on object");
				return 1;
			}
	}

	std::list<Contact*> contacts;
	std::list<Joint*> joints;

	bool freeChainForces = false;
	for(int c=0; c<hand->getNumChains(); c++) {
		//for now, we look at all contacts
		std::list<Contact*> chainContacts = hand->getChain(c)->getContacts(object);
		contacts.insert(contacts.end(), chainContacts.begin(), chainContacts.end());
		if (!chainContacts.empty()) {
			std::list<Joint*> chainJoints = hand->getChain(c)->getJoints();
			joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
		} else {
			//the chain has no contacts
			//check if any joint forces are not 0
			Matrix chainTau = hand->getChain(c)->jointTorquesVector(robotTau);
			//torque units should be N * 1.0e6 * mm
			if (chainTau.absMax() > 1.0e3) {
				DBGA("Joint torque " << chainTau.absMax() << " on chain " << c 
									 << " with no contacts");
				freeChainForces = true;
			}
		}
	}
	//if there are no contacts, nothing to compute!
	if (contacts.empty()) return 0;

	//assemble the joint forces matrix
	Matrix tau((int)joints.size(), 1);
	int jc; std::list<Joint*>::iterator jit;
	for (jc=0, jit = joints.begin(); jit!=joints.end(); jc++,jit++) {
		tau.elem(jc,0) = robotTau.elem( (*jit)->getNum(), 0 );
	}
	//if all the joint forces we care about are zero, do an early exit 
	//as zero contact forces are guaranteed to balance the chain
	//we should probably be able to use a much larger threshold here, if
	//units are what I think they are
	if (tau.absMax() < 1.0e-3) {
		return 0;
	}

	//if there are forces on chains with no contacts, we have no hope to balance them
	if (freeChainForces) {
		return 1;
	}

	Matrix J(contactJacobian(joints, contacts));
	Matrix D(Contact::frictionForceBlockMatrix(contacts));
	Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
	Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));

	//grasp map G = S*R*D
	Matrix G(graspMapMatrix(R, D));

	//left-hand equality matrix JTD = JTran * D
	Matrix JTran(J.transposed());
	Matrix JTD(JTran.rows(), D.cols());
	matrixMultiply(JTran, D, JTD);

	//matrix of zeroes for right-hand of friction inequality
	Matrix zeroes(Matrix::ZEROES<Matrix>(F.rows(), 1));

	//matrix of unknowns
	Matrix c_beta(D.cols(), 1);

	//bounds: all variables greater than 0
	Matrix lowerBounds(Matrix::ZEROES<Matrix>(D.cols(),1));
	Matrix upperBounds(Matrix::MAX_VECTOR(D.cols()));

	//solve QP
	double objVal;
	int result = factorizedQPSolver(G, JTD, tau, F, zeroes, lowerBounds, upperBounds, 
									c_beta, &objVal);
	if (result) {
		if( result > 0) {
			DBGP("Grasp: problem unfeasible");
		} else {
			DBGA("Grasp: QP solver error");
		}
		return result;
	}

	//retrieve contact wrenchs in local contact coordinate systems
	Matrix cWrenches(D.rows(), 1);
	matrixMultiply(D, c_beta, cWrenches);
	DBGP("Contact wrenches:\n" << cWrenches);

	//compute wrenches relative to object origin and expressed in world coordinates
	Matrix objectWrenches(R.rows(), cWrenches.cols());
	matrixMultiply(R, cWrenches, objectWrenches);
	DBGP("Object wrenches:\n" << objectWrenches);

	//display them on the contacts and accumulate them on the object
	displayContactWrenches(&contacts, cWrenches);
	accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);

	//simple sanity check: JT * c = tau
	Matrix fCheck(tau.rows(), 1);
	matrixMultiply(JTran, cWrenches, fCheck);
	for (int j=0; j<tau.rows(); j++) {
		//I am not sure this works well for universal and ball joints
		double err = fabs(tau.elem(j, 0) - fCheck.elem(j,0));
		//take error as a percentage of desired force, if force is non-zero
		if ( fabs(tau.elem(j,0)) > 1.0e-2) {
			err = err / fabs(tau.elem(j, 0));
		} else {
			//for zero desired torque, out of thin air we pull an error threshold of 1.0e2
			//which is 0.1% of the normal range of torques at 1.0e6
			if (err < 1.0e2) err = 0;
		}
		// 0.1% error is considered too much
		if (  err > 1.0e-1) {
			DBGA("Desired torque not obtained on joint " << j << ", error " << err << 
				" out of " << fabs(tau.elem(j, 0)) );
			return -1;
		}
	}

	//complex sanity check: is object force same as QP optimization result?
	//this is only expected to work if all contacts are on the same object
	double* extWrench = object->getExtWrenchAcc();
	vec3 force(extWrench[0], extWrench[1], extWrench[2]);
	vec3 torque(extWrench[3], extWrench[4], extWrench[5]);
	//take into account the scaling that has taken place
	double wrenchError = objVal*1.0e-6 - (force.len_sq() + torque.len_sq()) * 1.0e6;
	//units here are N * 1.0e-6; errors should be in the range on miliN
	if (wrenchError > 1.0e3) {
		DBGA("Wrench sanity check error: " << wrenchError);
		return -1;
	}
	return 0;
}
void dgWorldDynamicUpdate::CalculateClusterReactionForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep, dgFloat32 maxAccNorm) const
{
	dTimeTrackerEvent(__FUNCTION__);
	dgWorld* const world = (dgWorld*) this;
	const dgInt32 bodyCount = cluster->m_bodyCount;
	//	const dgInt32 jointCount = island->m_jointCount;
	const dgInt32 jointCount = cluster->m_activeJointCount;

	dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart];
	dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*)&world->m_bodiesMemory[0];
	dgJointInfo* const constraintArrayPtr = (dgJointInfo*)&world->m_jointsMemory[0];

	dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart];
	dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart];
	dgJacobianMatrixElement* const matrixRow = &m_solverMemory.m_jacobianBuffer[cluster->m_rowsStart];

	const dgInt32 derivativesEvaluationsRK4 = 4;
	dgFloat32 invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f);
	dgFloat32 invStepRK = (dgFloat32(1.0f) / dgFloat32(derivativesEvaluationsRK4));
	dgFloat32 timestepRK = timestep * invStepRK;
	dgFloat32 invTimestepRK = invTimestep * dgFloat32(derivativesEvaluationsRK4);
	dgAssert(bodyArray[0].m_body == world->m_sentinelBody);

	dgVector speedFreeze2(world->m_freezeSpeed2 * dgFloat32(0.1f));
	dgVector freezeOmega2(world->m_freezeOmega2 * dgFloat32(0.1f));

	dgJointAccelerationDecriptor joindDesc;
	joindDesc.m_timeStep = timestepRK;
	joindDesc.m_invTimeStep = invTimestepRK;
	joindDesc.m_firstPassCoefFlag = dgFloat32(0.0f);

	dgInt32 skeletonCount = 0;
	dgInt32 skeletonMemorySizeInBytes = 0;
	dgInt32 lru = dgAtomicExchangeAndAdd(&dgSkeletonContainer::m_lruMarker, 1);
	dgSkeletonContainer* skeletonArray[DG_MAX_SKELETON_JOINT_COUNT];
	dgInt32 memorySizes[DG_MAX_SKELETON_JOINT_COUNT];
	for (dgInt32 i = 1; i < bodyCount; i++) {
		dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
		dgSkeletonContainer* const container = body->GetSkeleton();
		if (container && (container->m_lru != lru)) {
			container->m_lru = lru;
			memorySizes[skeletonCount] = container->GetMemoryBufferSizeInBytes(constraintArray, matrixRow);
			skeletonMemorySizeInBytes += memorySizes[skeletonCount];
			skeletonArray[skeletonCount] = container;
			skeletonCount++;
			dgAssert(skeletonCount < dgInt32(sizeof(skeletonArray) / sizeof(skeletonArray[0])));
		}
	}

	dgInt8* const skeletonMemory = (dgInt8*)dgAlloca(dgVector, skeletonMemorySizeInBytes / sizeof(dgVector));
	dgAssert((dgInt64(skeletonMemory) & 0x0f) == 0);

	skeletonMemorySizeInBytes = 0;
	for (dgInt32 i = 0; i < skeletonCount; i++) {
		skeletonArray[i]->InitMassMatrix(constraintArray, matrixRow, &skeletonMemory[skeletonMemorySizeInBytes]);
		skeletonMemorySizeInBytes += memorySizes[i];
	}

	const dgInt32 passes = world->m_solverMode;
	for (dgInt32 step = 0; step < derivativesEvaluationsRK4; step++) {

		for (dgInt32 i = 0; i < jointCount; i++) {
			dgJointInfo* const jointInfo = &constraintArray[i];
			dgConstraint* const constraint = jointInfo->m_joint;
			joindDesc.m_rowsCount = jointInfo->m_pairCount;
			joindDesc.m_rowMatrix = &matrixRow[jointInfo->m_pairStart];
			constraint->JointAccelerations(&joindDesc);
		}
		joindDesc.m_firstPassCoefFlag = dgFloat32(1.0f);

		dgFloat32 accNorm(maxAccNorm * dgFloat32(2.0f));
		for (dgInt32 i = 0; (i < passes) && (accNorm > maxAccNorm); i++) {
			accNorm = dgFloat32(0.0f);
			for (dgInt32 j = 0; j < jointCount; j++) {
				dgJointInfo* const jointInfo = &constraintArray[j];
				dgFloat32 accel = CalculateJointForceGaussSeidel(jointInfo, bodyArray, internalForces, matrixRow, maxAccNorm);
				accNorm = (accel > accNorm) ? accel : accNorm;
			}
		}
		for (dgInt32 j = 0; j < skeletonCount; j++) {
			skeletonArray[j]->CalculateJointForce(constraintArray, bodyArray, internalForces, matrixRow);
		}

		if (timestepRK != dgFloat32(0.0f)) {
			dgVector timestep4(timestepRK);
			for (dgInt32 i = 1; i < bodyCount; i++) {
				dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
				dgAssert(body->m_index == i);
				if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
					const dgJacobian& forceAndTorque = internalForces[i];
					dgVector force(body->m_externalForce + forceAndTorque.m_linear);
					dgVector torque(body->m_externalTorque + forceAndTorque.m_angular);

					dgVector velocStep((force.Scale4(body->m_invMass.m_w)) * timestep4);
					dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4);
					body->m_veloc += velocStep;
					body->m_omega += omegaStep;

					dgAssert(body->m_veloc.m_w == dgFloat32(0.0f));
					dgAssert(body->m_omega.m_w == dgFloat32(0.0f));
				}
			}
		} else {
			for (dgInt32 i = 1; i < bodyCount; i++) {
				dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
				const dgVector& linearMomentum = internalForces[i].m_linear;
				const dgVector& angularMomentum = internalForces[i].m_angular;

				body->m_veloc += linearMomentum.Scale4(body->m_invMass.m_w);
				body->m_omega += body->m_invWorldInertiaMatrix.RotateVector(angularMomentum);
			}
		}
	}

	dgInt32 hasJointFeeback = 0;
	if (timestepRK != dgFloat32(0.0f)) {
		for (dgInt32 i = 0; i < jointCount; i++) {
			dgJointInfo* const jointInfo = &constraintArray[i];
			dgConstraint* const constraint = jointInfo->m_joint;

			const dgInt32 first = jointInfo->m_pairStart;
			const dgInt32 count = jointInfo->m_pairCount;

			for (dgInt32 j = 0; j < count; j++) {
				dgJacobianMatrixElement* const row = &matrixRow[j + first];
				dgFloat32 val = row->m_force;
				dgAssert(dgCheckFloat(val));
				row->m_jointFeebackForce->m_force = val;
				row->m_jointFeebackForce->m_impact = row->m_maxImpact * timestepRK;
			}
			hasJointFeeback |= (constraint->m_updaFeedbackCallback ? 1 : 0);
		}

		const dgVector invTime(invTimestep);
		const dgVector maxAccNorm2(maxAccNorm * maxAccNorm);
		for (dgInt32 i = 1; i < bodyCount; i++) {
			dgBody* const body = bodyArray[i].m_body;
			CalculateNetAcceleration(body, invTime, maxAccNorm2);
		}
		if (hasJointFeeback) {
			for (dgInt32 i = 0; i < jointCount; i++) {
				if (constraintArray[i].m_joint->m_updaFeedbackCallback) {
					constraintArray[i].m_joint->m_updaFeedbackCallback(*constraintArray[i].m_joint, timestep, threadID);
				}
			}
		}
	} else {
		for (dgInt32 i = 1; i < bodyCount; i++) {
			dgBody* const body = bodyArray[i].m_body;
			dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI));
			body->m_accel = dgVector::m_zero;
			body->m_alpha = dgVector::m_zero;
		}
	}
}
Ejemplo n.º 30
-16
StarshipAI::StarshipAI(SimObject* s)
: ShipAI(s), sub_select_time(0), subtarget(0), tgt_point_defense(false)
{
	ai_type = STARSHIP;

	// signifies this ship is a dead hulk:
	if (ship && ship->Design()->auto_roll < 0) {
		Point torque(rand()-16000, rand()-16000, rand()-16000);
		torque.Normalize();
		torque *= ship->Mass() / 10;

		ship->SetFLCSMode(0);
		if (ship->GetFLCS())
		ship->GetFLCS()->PowerOff();

		ship->ApplyTorque(torque);
		ship->SetVelocity(RandomDirection() * Random(20, 50));

		for (int i = 0; i < 64; i++) {
			Weapon* w = ship->GetWeaponByIndex(i+1);
			if (w)
			w->DrainPower(0);
			else
			break;
		}
	}

	else {
		tactical = new(__FILE__,__LINE__) StarshipTacticalAI(this);
	}

	sub_select_time      = Game::GameTime() + (DWORD) Random(0, 2000);
	point_defense_time   = sub_select_time;
}