void mtsTeleOperationECM::TransitionEnabled(void)
{
std::string armState;
// check ECM state
mECM.GetCurrentState(armState);
if (armState != "READY") {
mInterface->SendWarning(this->GetName() + ": ECM state has changed to [" + armState + "]");
mTeleopState.SetDesiredState("DISABLED");
}
// check mtml state
mMTML.GetCurrentState(armState);
if (armState != "READY") {
mInterface->SendWarning(this->GetName() + ": MTML state has changed to [" + armState + "]");
mTeleopState.SetDesiredState("DISABLED");
}
// check mtmr state
mMTMR.GetCurrentState(armState);
if (armState != "READY") {
mInterface->SendWarning(this->GetName() + ": MTMR state has changed to [" + armState + "]");
mTeleopState.SetDesiredState("DISABLED");
}
if (mTeleopState.DesiredStateIsNotCurrent()) {
SetFollowing(false);
mTeleopState.SetCurrentState(mTeleopState.DesiredState());
}
}
// This function lets a slave refollow his master
bool CreatureLinkingHolder::TryFollowMaster(Creature* pCreature)
{
CreatureLinkingInfo const* pInfo = sCreatureLinkingMgr.GetLinkedTriggerInformation(pCreature);
if (!pInfo || !(pInfo->linkingFlag & FLAG_FOLLOW))
return false;
Creature* pMaster = NULL;
if (pInfo->mapId != INVALID_MAP_ID) // entry case
{
BossGuidMapBounds finds = m_masterGuid.equal_range(pInfo->masterId);
for (BossGuidMap::iterator itr = finds.first; itr != finds.second; ++itr)
{
pMaster = pCreature->GetMap()->GetCreature(itr->second);
if (pMaster && IsSlaveInRangeOfBoss(pCreature, pMaster, pInfo->searchRange))
break;
}
}
else // guid case
{
CreatureData const* masterData = sObjectMgr.GetCreatureData(pInfo->masterDBGuid);
CreatureInfo const* cInfo = ObjectMgr::GetCreatureTemplate(masterData->id);
pMaster = pCreature->GetMap()->GetCreature(ObjectGuid(cInfo->GetHighGuid(), cInfo->Entry, pInfo->masterDBGuid));
}
if (pMaster && pMaster->isAlive())
{
SetFollowing(pCreature, pMaster);
return true;
}
return false;
}
// Helper function, to process a single slave
void CreatureLinkingHolder::ProcessSlave(CreatureLinkingEvent eventType, Creature* pSource, uint32 flag, Creature* pSlave, Unit* pEnemy)
{
switch (eventType)
{
case LINKING_EVENT_AGGRO:
if (flag & FLAG_AGGRO_ON_AGGRO)
{
if (pSlave->IsControlledByPlayer())
return;
if (pSlave->isInCombat())
pSlave->SetInCombatWith(pEnemy);
else
pSlave->AI()->AttackStart(pEnemy);
}
break;
case LINKING_EVENT_EVADE:
if (flag & FLAG_DESPAWN_ON_EVADE && pSlave->isAlive())
pSlave->ForcedDespawn();
if (flag & FLAG_RESPAWN_ON_EVADE && !pSlave->isAlive())
pSlave->Respawn();
break;
case LINKING_EVENT_DIE:
if (flag & FLAG_SELFKILL_ON_DEATH && pSlave->isAlive())
pSlave->DealDamage(pSlave, pSlave->GetHealth(), NULL, DIRECT_DAMAGE, SPELL_SCHOOL_MASK_NORMAL, NULL, false);
if (flag & FLAG_DESPAWN_ON_DEATH && pSlave->isAlive())
pSlave->ForcedDespawn();
if (flag & FLAG_RESPAWN_ON_DEATH && !pSlave->isAlive())
pSlave->Respawn();
break;
case LINKING_EVENT_RESPAWN:
if (flag & FLAG_RESPAWN_ON_RESPAWN)
{
// Additional check to prevent endless loops (in case whole group respawns on first respawn)
if (!pSlave->isAlive() && pSlave->GetRespawnTime() > time(NULL))
pSlave->Respawn();
}
else if (flag & FLAG_DESPAWN_ON_RESPAWN && pSlave->isAlive())
pSlave->ForcedDespawn();
if (flag & FLAG_FOLLOW && pSlave->isAlive() && !pSlave->isInCombat())
SetFollowing(pSlave, pSource);
break;
case LINKING_EVENT_DESPAWN:
if (flag & FLAG_DESPAWN_ON_DESPAWN && !pSlave->IsDespawned())
pSlave->ForcedDespawn();
break;
}
}
// This function lets a slave refollow his master
bool CreatureLinkingHolder::TryFollowMaster(Creature* pCreature)
{
CreatureLinkingInfo const* pInfo = sCreatureLinkingMgr.GetLinkedTriggerInformation(pCreature);
if (!pInfo || !(pInfo->linkingFlag & FLAG_FOLLOW))
return false;
BossGuidMapBounds finds = m_masterGuid.equal_range(pInfo->masterId);
for (BossGuidMap::iterator itr = finds.first; itr != finds.second; ++itr)
{
Creature* pMaster = pCreature->GetMap()->GetCreature(itr->second);
if (pMaster && pMaster->isAlive() && IsSlaveInRangeOfBoss(pCreature, pMaster, pInfo->searchRange))
{
SetFollowing(pCreature, pMaster);
return true;
}
}
return false;
}
void mtsTeleOperationECM::Clutch(const bool & clutch)
{
// if the teleoperation is activated
if (clutch) {
SetFollowing(false);
mInterface->SendStatus(this->GetName() + ": console clutch pressed");
// set MTMs in effort mode, no force applied but gravity and locked orientation
prmForceCartesianSet wrench;
mMTML.SetWrenchBody(wrench);
mMTML.SetGravityCompensation(true);
mMTML.LockOrientation(mMTML.PositionCartesianCurrent.Position().Rotation());
mMTMR.SetWrenchBody(wrench);
mMTMR.SetGravityCompensation(true);
mMTMR.LockOrientation(mMTMR.PositionCartesianCurrent.Position().Rotation());
} else {
mIsClutched = false;
mInterface->SendStatus(this->GetName() + ": console clutch released");
mTeleopState.SetCurrentState("SETTING_ARMS_STATE");
}
}
// Function to process actions for linked NPCs
void CreatureLinkingHolder::DoCreatureLinkingEvent(CreatureLinkingEvent eventType, Creature* pSource, Unit* pEnemy /* = NULL*/)
{
// This check will be needed in reload case
if (!sCreatureLinkingMgr.IsLinkedEventTrigger(pSource))
return;
// Ignore atypic behaviour
if (pSource->IsControlledByPlayer())
return;
if (eventType == LINKING_EVENT_AGGRO && !pEnemy)
return;
uint32 eventFlagFilter = 0;
uint32 reverseEventFlagFilter = 0;
switch (eventType)
{
case LINKING_EVENT_AGGRO: eventFlagFilter = EVENT_MASK_ON_AGGRO; reverseEventFlagFilter = FLAG_TO_AGGRO_ON_AGGRO; break;
case LINKING_EVENT_EVADE: eventFlagFilter = EVENT_MASK_ON_EVADE; reverseEventFlagFilter = FLAG_TO_RESPAWN_ON_EVADE; break;
case LINKING_EVENT_DIE: eventFlagFilter = EVENT_MASK_ON_DIE; reverseEventFlagFilter = 0; break;
case LINKING_EVENT_RESPAWN: eventFlagFilter = EVENT_MASK_ON_RESPAWN; reverseEventFlagFilter = FLAG_FOLLOW; break;
}
// Process Slaves (by entry)
HolderMapBounds bounds = m_holderMap.equal_range(pSource->GetEntry());
for (HolderMap::iterator itr = bounds.first; itr != bounds.second; ++itr)
ProcessSlaveGuidList(eventType, pSource, itr->second.linkingFlag & eventFlagFilter, itr->second.searchRange, itr->second.linkedGuids, pEnemy);
// Process Slaves (by guid)
bounds = m_holderGuidMap.equal_range(pSource->GetGUIDLow());
for (HolderMap::iterator itr = bounds.first; itr != bounds.second; ++itr)
ProcessSlaveGuidList(eventType, pSource, itr->second.linkingFlag & eventFlagFilter, itr->second.searchRange, itr->second.linkedGuids, pEnemy);
// Process Master
if (CreatureLinkingInfo const* pInfo = sCreatureLinkingMgr.GetLinkedTriggerInformation(pSource))
{
if (pInfo->linkingFlag & reverseEventFlagFilter)
{
Creature* pMaster = NULL;
if (pInfo->mapId != INVALID_MAP_ID) // entry case
{
BossGuidMapBounds finds = m_masterGuid.equal_range(pInfo->masterId);
for (BossGuidMap::iterator itr = finds.first; itr != finds.second; ++itr)
{
pMaster = pSource->GetMap()->GetCreature(itr->second);
if (pMaster && IsSlaveInRangeOfBoss(pSource, pMaster, pInfo->searchRange))
break;
}
}
else // guid case
{
CreatureData const* masterData = sObjectMgr.GetCreatureData(pInfo->masterDBGuid);
CreatureInfo const* cInfo = ObjectMgr::GetCreatureTemplate(masterData->id);
pMaster = pSource->GetMap()->GetCreature(ObjectGuid(cInfo->GetHighGuid(), cInfo->Entry, pInfo->masterDBGuid));
}
if (pMaster)
{
switch (eventType)
{
case LINKING_EVENT_AGGRO:
if (pMaster->IsControlledByPlayer())
return;
if (pMaster->isInCombat())
pMaster->SetInCombatWith(pEnemy);
else
pMaster->AI()->AttackStart(pEnemy);
break;
case LINKING_EVENT_EVADE:
if (!pMaster->isAlive())
pMaster->Respawn();
break;
case LINKING_EVENT_RESPAWN:
if (pMaster->isAlive())
SetFollowing(pSource, pMaster);
}
}
}
}
}
void mtsTeleOperationECM::EnterEnabled(void)
{
// set cartesian effort parameters
mMTML.SetWrenchBodyOrientationAbsolute(true);
mMTML.LockOrientation(mMTML.PositionCartesianCurrent.Position().Rotation());
mMTMR.SetWrenchBodyOrientationAbsolute(true);
mMTMR.LockOrientation(mMTMR.PositionCartesianCurrent.Position().Rotation());
// initial state for MTM force feedback
// -1- initial distance between MTMs
vct3 vectorLR;
vectorLR.DifferenceOf(mMTMR.PositionCartesianCurrent.Position().Translation(),
mMTML.PositionCartesianCurrent.Position().Translation());
// -2- mid-point, aka center of image
mInitial.C.SumOf(mMTMR.PositionCartesianCurrent.Position().Translation(),
mMTML.PositionCartesianCurrent.Position().Translation());
mInitial.C.Multiply(0.5);
// -3- image up vector
mInitial.Up.CrossProductOf(vectorLR, mInitial.C);
mInitial.Up.NormalizedSelf();
// -4- width of image, depth of arms wrt image plan
vct3 side;
side.CrossProductOf(mInitial.C, mInitial.Up);
side.NormalizedSelf();
mInitial.w = 0.5 * vctDotProduct(side, vectorLR);
mInitial.d = 0.5 * vctDotProduct(mInitial.C.Normalized(), vectorLR);
// projections
mInitial.Lr.Assign(mInitial.C[0], 0, mInitial.C[2]);
mInitial.Lr.NormalizedSelf();
mInitial.Ud.Assign(0, mInitial.C[1], mInitial.C[2]);
mInitial.Ud.NormalizedSelf();
mInitial.Cw.Assign(mInitial.Up[0], mInitial.Up[1], 0);
mInitial.Cw.NormalizedSelf();
mInitial.ECMPositionJoint = mECM.StateJointDesired.Position();
// -5- store current rotation matrix for MTML, MTMR, and ECM
vctEulerZYXRotation3 eulerAngles;
eulerAngles.Assign(mInitial.ECMPositionJoint[3], mInitial.ECMPositionJoint[0], mInitial.ECMPositionJoint[1]);
vctEulerToMatrixRotation3(eulerAngles, mInitial.ECMRotEuler);
mInitial.MTMLRot = mMTML.PositionCartesianCurrent.Position().Rotation();
mInitial.MTMRRot = mMTMR.PositionCartesianCurrent.Position().Rotation();
#if 0
std::cerr << CMN_LOG_DETAILS << std::endl
<< "L: " << mMTML.PositionCartesianCurrent.Position().Translation() << std::endl
<< "R: " << mMTMR.PositionCartesianCurrent.Position().Translation() << std::endl
<< "C: " << mInitial.C << std::endl
<< "Up: " << mInitial.Up << std::endl
<< "w: " << mInitial.w << std::endl
<< "d: " << mInitial.d << std::endl
<< "Si: " << side << std::endl;
#endif
mInitial.ECMPositionJoint = mECM.StateJointDesired.Position();
// check if by any chance the clutch pedal is pressed
if (mIsClutched) {
Clutch(true);
} else {
SetFollowing(true);
}
}
void mtsTeleOperationECM::RunAllStates(void)
{
mtsExecutionResult executionResult;
// get MTML Cartesian position/velocity
executionResult = mMTML.GetPositionCartesian(mMTML.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTML.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
mInterface->SendError(this->GetName() + ": unable to get cartesian position from MTML");
this->SetDesiredState("DISABLED");
}
executionResult = mMTML.GetVelocityCartesian(mMTML.VelocityCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTML.GetVelocityCartesian failed \""
<< executionResult << "\"" << std::endl;
mInterface->SendError(this->GetName() + ": unable to get cartesian velocity from MTML");
this->SetDesiredState("DISABLED");
}
// get MTMR Cartesian position
executionResult = mMTMR.GetPositionCartesian(mMTMR.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTMR.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
mInterface->SendError(this->GetName() + ": unable to get cartesian position from MTMR");
this->SetDesiredState("DISABLED");
}
executionResult = mMTMR.GetVelocityCartesian(mMTMR.VelocityCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to MTMR.GetVelocityCartesian failed \""
<< executionResult << "\"" << std::endl;
mInterface->SendError(this->GetName() + ": unable to get cartesian velocity from MTMR");
this->SetDesiredState("DISABLED");
}
// get ECM Cartesian position for GUI
executionResult = mECM.GetPositionCartesian(mECM.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to ECM.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
mInterface->SendError(this->GetName() + ": unable to get cartesian position from ECM");
this->SetDesiredState("DISABLED");
}
// for motion computation
executionResult = mECM.GetStateJointDesired(mECM.StateJointDesired);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to ECM.GetStateJointDesired failed \""
<< executionResult << "\"" << std::endl;
mInterface->SendError(this->GetName() + ": unable to get joint state from ECM");
this->SetDesiredState("DISABLED");
}
// check if anyone wanted to disable anyway
if ((mTeleopState.DesiredState() == "DISABLED")
&& (mTeleopState.CurrentState() != "DISABLED")) {
SetFollowing(false);
mTeleopState.SetCurrentState("DISABLED");
return;
}
// monitor state of arms if needed
if ((mTeleopState.CurrentState() != "DISABLED")
&& (mTeleopState.CurrentState() != "SETTING_ARMS_STATE")) {
std::string armState;
mECM.GetDesiredState(armState);
if (armState != "READY") {
this->SetDesiredState("DISABLED");
mInterface->SendError(this->GetName() + ": ECM is not in state \"READY\" anymore");
}
mMTML.GetDesiredState(armState);
if (armState != "READY") {
this->SetDesiredState("DISABLED");
mInterface->SendError(this->GetName() + ": MTML is not in state \"READY\" anymore");
}
mMTMR.GetDesiredState(armState);
if (armState != "READY") {
this->SetDesiredState("DISABLED");
mInterface->SendError(this->GetName() + ": MTMR is not in state \"READY\" anymore");
}
}
}
void mtsTeleOperationECM::Startup(void)
{
CMN_LOG_CLASS_INIT_VERBOSE << "Startup" << std::endl;
SetScale(mScale);
SetFollowing(false);
}
