AngVelocity GyroscopeSensor::predictMeasurement(const Twist& linkVel)
{
AngVelocity angVel(0,0,0);
if(this->pimpl->parent_link_index>=0)
{
angVel = ((this->pimpl->link_H_sensor.inverse() * linkVel).getAngularVec3());
}
return(angVel);
}
void Foam::cfdemCloudIB::calcVelocityCorrection
(
volScalarField& p,
volVectorField& U,
volScalarField& phiIB,
volScalarField& voidfraction
)
{
label cellI=0;
vector uParticle(0,0,0);
vector rVec(0,0,0);
vector velRot(0,0,0);
vector angVel(0,0,0);
for(int index=0; index< numberOfParticles(); index++)
{
//if(regionM().inRegion()[index][0])
//{
for(int subCell=0;subCell<voidFractionM().cellsPerParticle()[index][0];subCell++)
{
//Info << "subCell=" << subCell << endl;
cellI = cellIDs()[index][subCell];
if (cellI >= 0)
{
// calc particle velocity
for(int i=0;i<3;i++) rVec[i]=U.mesh().C()[cellI][i]-position(index)[i];
for(int i=0;i<3;i++) angVel[i]=angularVelocities()[index][i];
velRot=angVel^rVec;
for(int i=0;i<3;i++) uParticle[i] = velocities()[index][i]+velRot[i];
// impose field velocity
U[cellI]=(1-voidfractions_[index][subCell])*uParticle+voidfractions_[index][subCell]*U[cellI];
}
}
//}
}
// make field divergence free - set reference value in case it is needed
fvScalarMatrix phiIBEqn
(
fvm::laplacian(phiIB) == fvc::div(U) + fvc::ddt(voidfraction)
);
if(phiIB.needReference())
{
phiIBEqn.setReference(pRefCell_, pRefValue_);
}
phiIBEqn.solve();
U=U-fvc::grad(phiIB);
U.correctBoundaryConditions();
// correct the pressure as well
p=p+phiIB/U.mesh().time().deltaT(); // do we have to account for rho here?
p.correctBoundaryConditions();
}
Twist Axis::getRotationTwist(const double dtheta) const
{
Twist ret;
Eigen::Map<Eigen::Vector3d> linVel(ret.getLinearVec3().data());
Eigen::Map<Eigen::Vector3d> angVel(ret.getAngularVec3().data());
Eigen::Map<const Eigen::Vector3d> dir(direction.data());
Eigen::Map<const Eigen::Vector3d> orig(origin.data());
linVel = dtheta*(orig.cross(dir));
angVel = dir*dtheta;
return ret;
}
//-----------------------------------------------------------------------------
// Purpose:
//
//
//-----------------------------------------------------------------------------
void CWeaponMolotov::ThrowMolotov( const Vector &vecSrc, const Vector &vecVelocity)
{
CGrenade_Molotov *pMolotov = (CGrenade_Molotov*)Create( "grenade_molotov", vecSrc, vec3_angle, GetOwner() );
if (!pMolotov)
{
Msg("Couldn't make molotov!\n");
return;
}
pMolotov->SetAbsVelocity( vecVelocity );
// Tumble through the air
QAngle angVel( random->RandomFloat ( -100, -500 ), random->RandomFloat ( -100, -500 ), random->RandomFloat ( -100, -500 ) );
pMolotov->SetLocalAngularVelocity( angVel );
pMolotov->SetThrower( GetOwner() );
pMolotov->SetOwnerEntity( ((CBaseEntity*)GetOwner()) );
}
void Leapfrog::leapfrogAsphericalRotate(const shared_ptr<Node>& node, const Vector3r& M){
Quaternionr& ori(node->ori); DemData& dyn(node->getData<DemData>());
Vector3r& angMom(dyn.angMom); Vector3r& angVel(dyn.angVel); const Vector3r& inertia(dyn.inertia);
// initialize angular momentum; it is in local coordinates, therefore angVel must be rotated
if(isnan(angMom.minCoeff())){
angMom=dyn.inertia.asDiagonal()*node->ori.conjugate()*dyn.angVel;
}
Matrix3r A=ori.conjugate().toRotationMatrix(); // rotation matrix from global to local r.f.
const Vector3r l_n = angMom + dt/2 * M; // global angular momentum at time n
const Vector3r l_b_n = A*l_n; // local angular momentum at time n
const Vector3r angVel_b_n = (l_b_n.array()/inertia.array()).matrix(); // local angular velocity at time n
const Quaternionr dotQ_n=DotQ(angVel_b_n,ori); // dQ/dt at time n
const Quaternionr Q_half = ori + dt/2 * dotQ_n; // Q at time n+1/2
angMom+=dt*M; // global angular momentum at time n+1/2
const Vector3r l_b_half = A*angMom; // local angular momentum at time n+1/2
Vector3r angVel_b_half = (l_b_half.array()/inertia.array()).matrix(); // local angular velocity at time n+1/2
const Quaternionr dotQ_half=DotQ(angVel_b_half,Q_half); // dQ/dt at time n+1/2
ori=ori+dt*dotQ_half; // Q at time n+1
angVel=ori*angVel_b_half; // global angular velocity at time n+1/2
ori.normalize();
}
void CASW_Laser_Mine::StartSpawnFlipping( Vector vecStart, Vector vecEnd, QAngle angEnd, float flDuration )
{
m_vecSpawnFlipStartPos = vecStart;
m_vecSpawnFlipEndPos = vecEnd;
m_angSpawnFlipEndAngle = angEnd;
StopFollowingEntity( );
SetMoveType( MOVETYPE_NOCLIP );
SetGroundEntity( NULL );
SetTouch(NULL);
SetSolidFlags( 0 );
m_flSpawnFlipStartTime = gpGlobals->curtime;
m_flSpawnFlipEndTime = gpGlobals->curtime + flDuration;
SetContextThink( &CASW_Laser_Mine::SpawnFlipThink, gpGlobals->curtime, s_pLaserMineSpawnFlipThink );
SetAbsAngles( m_angSpawnFlipEndAngle );
QAngle angVel( 0, random->RandomFloat ( 200, 400 ), 0 );
SetLocalAngularVelocity( angVel );
m_bIsSpawnFlipping = true;
}
// this is the think that flips the weapon into the world when it is spawned
void CASW_Laser_Mine::SpawnFlipThink()
{
// we are still flagged as spawn flipping in the air
if ( m_bIsSpawnFlipping == false )
{
// we get here if we spawned, but haven't started spawn flipping yet, please try again!
SetContextThink( &CASW_Laser_Mine::SpawnFlipThink, gpGlobals->curtime, s_pLaserMineSpawnFlipThink );
return;
}
// when we should hit the ground
float flEndTime = m_flSpawnFlipEndTime;
// the total time it takes for us to flip
float flFlipTime = flEndTime - m_flSpawnFlipStartTime;
float flFlipProgress = ( gpGlobals->curtime - m_flSpawnFlipStartTime ) / flFlipTime;
flFlipProgress = clamp( flFlipProgress, 0.0f, 2.5f );
if ( !m_bIsSpawnLanded )
{
// high gravity, it looks more satisfying
float flGravity = 2200;
float flInitialZVelocity = (m_vecSpawnFlipEndPos.z - m_vecSpawnFlipStartPos.z)/flFlipTime + (flGravity/2) * flFlipTime;
float flZVelocity = flInitialZVelocity - flGravity * (gpGlobals->curtime - m_flSpawnFlipStartTime);
float flXDiff = (m_vecSpawnFlipEndPos.x - m_vecSpawnFlipStartPos.x) / flFlipTime;
float flYDiff = (m_vecSpawnFlipEndPos.y - m_vecSpawnFlipStartPos.y) / flFlipTime;
Vector vecVelocity( flXDiff, flYDiff, flZVelocity );
SetAbsVelocity( vecVelocity );
// angular velocity
QAngle angCurAngVel = GetLocalAngularVelocity();
float flXAngDiff = 360 / flFlipTime;
// keep the Y angular velocity that was given to it at the start (it's random)
SetLocalAngularVelocity( QAngle( flXAngDiff, angCurAngVel.y, 0 ) );
}
if ( flFlipProgress >= 1.0f )
{
if ( !m_bIsSpawnLanded )
{
Vector vecVelStop( 0,0,0 );
SetAbsVelocity( vecVelStop );
SetAbsOrigin( m_vecSpawnFlipEndPos );
QAngle angVel( 0, 0, 0 );
SetLocalAngularVelocity( angVel );
/*
// get the current angles of the item so we can use them to determine the final angles
QAngle angPrevAngles = GetAbsAngles();
float flYAngles = angPrevAngles.y;
QAngle angFlat( 0, flYAngles, 0 );
*/
SetAbsAngles( m_angSpawnFlipEndAngle );
EmitSound("ASW_Laser_Mine.Lay");
m_bIsSpawnLanded = true;
}
if ( flFlipProgress >= 2.5f )
{
SetContextThink( NULL, 0, s_pLaserMineSpawnFlipThink );
EmitSound("ASW_Mine.Lay");
m_bMineActive = true;
return;
}
}
SetContextThink( &CASW_Laser_Mine::SpawnFlipThink, gpGlobals->curtime, s_pLaserMineSpawnFlipThink );
}
void updateHaptics(void)
{
// angular velocit
cVector3d angVel(0,0,0);
// reset clock
cPrecisionClock clock;
clock.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// SIMULATION TIME
/////////////////////////////////////////////////////////////////////
// stop the simulation clock
clock.stop();
// read the time increment in seconds
double timeInterval = clock.getCurrentTimeSeconds();
// restart the simulation clock
clock.reset();
clock.start();
// update frequency counter
frequencyCounter.signal(1);
/////////////////////////////////////////////////////////////////////////
// SIMULATION
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
// HAPTIC FORCE COMPUTATION
/////////////////////////////////////////////////////////////////////
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
// get position of cursor in global coordinates
cVector3d toolPos = tool->getDeviceGlobalPos();
// get position of object in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute a vector from the center of mass of the object (point of rotation) to the tool
cVector3d v = cSub(toolPos, objectPos);
// compute angular acceleration based on the interaction forces
// between the tool and the object
cVector3d angAcc(0,0,0);
if (v.length() > 0.0)
{
// get the last force applied to the cursor in global coordinates
// we negate the result to obtain the opposite force that is applied on the
// object
cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);
// compute the effective force that contributes to rotating the object.
cVector3d force = toolForce - cProject(toolForce, v);
// compute the resulting torque
cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force));
// compute a torque to restore the face to its original position
cVector3d dirFace = object->getLocalRot().getCol0();
cVector3d dirTorque = cCross(dirFace, cVector3d(0, 1, 0));
dirTorque.mul(3.0);
torque.add(dirTorque);
cVector3d upFace = object->getLocalRot().getCol2();
cVector3d upTorque = cCross(upFace, cVector3d(0, 0, 1));
dirTorque.mul(3.0);
torque.add(upTorque);
// update rotational acceleration
const double INERTIA = 0.4;
angAcc = (1.0 / INERTIA) * torque;
}
// update rotational velocity
angVel.add(timeInterval * angAcc);
// set a threshold on the rotational velocity term
const double MAX_ANG_VEL = 10.0;
double vel = angVel.length();
if (vel > MAX_ANG_VEL)
{
angVel.mul(MAX_ANG_VEL / vel);
}
// add some damping too
const double DAMPING = 0.1;
angVel.mul(1.0 - DAMPING * timeInterval);
// if user switch is pressed, set velocity to zero
if (tool->getUserSwitch(0) == 1)
{
angVel.zero();
}
// compute the next rotation configuration of the object
if (angVel.length() > C_SMALL)
{
object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length());
}
}
// exit haptics thread
simulationFinished = true;
}
//------------------------------------------------------------------------------
Real AttitudeData::GetReal(Integer item)
{
if (mSpacecraft == NULL)
InitializeRefObjects();
Real epoch = mSpacecraft->GetEpoch();
// get the basics - cosine matrix, angular velocity, euler angle sequence
Rmatrix33 cosMat = mSpacecraft->GetAttitude(epoch);
// Some attitude models don't compute rates, so we only want to try to get
// rates if that is the data we need to return
Rvector3 angVel(0.0,0.0,0.0);
if (((item >= EULER_ANGLE_RATE_1) && (item <= EULER_ANGLE_RATE_3)) ||
((item >= ANGULAR_VELOCITY_X) && (item <= ANGULAR_VELOCITY_Z)))
{
angVel = mSpacecraft->GetAngularVelocity(epoch)
* GmatMathConstants::DEG_PER_RAD;
}
UnsignedIntArray seq = mSpacecraft->GetEulerAngleSequence();
Rvector3 euler;
if (item == DCM_11) return cosMat(0,0);
if (item == DCM_12) return cosMat(0,1);
if (item == DCM_13) return cosMat(0,2);
if (item == DCM_21) return cosMat(1,0);
if (item == DCM_22) return cosMat(1,1);
if (item == DCM_23) return cosMat(1,2);
if (item == DCM_31) return cosMat(2,0);
if (item == DCM_32) return cosMat(2,1);
if (item == DCM_33) return cosMat(2,2);
if (item == ANGULAR_VELOCITY_X) return angVel[0];
if (item == ANGULAR_VELOCITY_Y) return angVel[1];
if (item == ANGULAR_VELOCITY_Z) return angVel[2];
// do conversions if necessary
if ((item >= QUAT_1) && (item <= QUAT_4))
{
Rvector quat = AttitudeConversionUtility::ToQuaternion(cosMat);
return quat[item - QUAT_1];
}
if ((item >= EULER_ANGLE_1) && (item <= EULER_ANGLE_3))
{
euler = AttitudeConversionUtility::ToEulerAngles(cosMat,
(Integer) seq[0],
(Integer) seq[1],
(Integer) seq[2]) * GmatMathConstants::DEG_PER_RAD;
return euler[item - EULER_ANGLE_1];
}
// Dunn added conversion below with the comment that this
// is slightly hacked! We might want to change to a more
// obvious method of index control.
if ((item >= MRP_1) && (item <= MRP_3))
{
Rvector quat = AttitudeConversionUtility::ToQuaternion(cosMat);
Rvector3 mrp = AttitudeConversionUtility::ToMRPs(quat);
return mrp[item - MRP_1];
}
if ((item >= EULER_ANGLE_RATE_1) && (item <= EULER_ANGLE_RATE_3))
{
euler = AttitudeConversionUtility::ToEulerAngles(cosMat,
(Integer) seq[0],
(Integer) seq[1],
(Integer) seq[2]) * GmatMathConstants::DEG_PER_RAD;
Rvector3 eulerRates = AttitudeConversionUtility::ToEulerAngleRates(
angVel * GmatMathConstants::RAD_PER_DEG,
euler * GmatMathConstants::RAD_PER_DEG,
(Integer) seq[0],
(Integer) seq[1],
(Integer) seq[2]) * GmatMathConstants::DEG_PER_RAD;
return eulerRates[item - EULER_ANGLE_RATE_1];
}
// otherwise, there is an error
throw ParameterException
("AttitudeData::GetReal() Not readable or unknown item id: " +
GmatRealUtil::ToString(item));
}
