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();
}
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;
}
}
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;
}
}
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]);
}
}
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));
}
}
}
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;
}
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();
}
void Thruster::rollRight()
{
torque(false);
}
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));
}
//---------------------------------------------------------
//---------------------------------------------------------
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';
}
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);
}
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);
}
}
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;
}
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;
}
}
}
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;
}