bool
SoccerBase::MoveAndRotateAgent(shared_ptr<Transform> agent_aspect, const Vector3f& pos, float angle)
{
Vector3f agentPos = agent_aspect->GetWorldTransform().Pos();
shared_ptr<Transform> parent = shared_dynamic_cast<Transform>
(agent_aspect->FindParentSupportingClass<Transform>().lock());
if (parent.get() == 0)
{
agent_aspect->GetLog()->Error() << "(MoveAndRotateAgent) ERROR: can't get parent node.\n";
return false;
}
Leaf::TLeafList leafList;
parent->ListChildrenSupportingClass<Body>(leafList, true);
if (leafList.size() == 0)
{
agent_aspect->GetLog()->Error()
<< "(MoveAndRotateAgent) ERROR: agent aspect doesn't have "
<< "children of type Body\n";
return false;
}
shared_ptr<Body> body;
GetAgentBody(agent_aspect, body);
Matrix bodyR = body->GetRotation();
bodyR.InvertRotationMatrix();
Matrix mat;
mat.RotationZ(gDegToRad(angle));
mat *= bodyR;
Leaf::TLeafList::iterator iter = leafList.begin();
// move all child bodies
for (iter;
iter != leafList.end();
++iter
)
{
shared_ptr<Body> childBody =
shared_dynamic_cast<Body>(*iter);
Vector3f childPos = childBody->GetPosition();
Matrix childR = childBody->GetRotation();
childR = mat*childR;
childBody->SetPosition(pos + mat.Rotate(childPos-agentPos));
childBody->SetVelocity(Vector3f(0,0,0));
childBody->SetAngularVelocity(Vector3f(0,0,0));
childBody->SetRotation(childR);
}
return true;
}
bool
SoccerBase::MoveAgent(boost::shared_ptr<Transform> agent_aspect, const Vector3f& pos)
{
Vector3f agentPos = agent_aspect->GetWorldTransform().Pos();
boost::shared_ptr<Transform> parent = shared_dynamic_cast<Transform>
(agent_aspect->FindParentSupportingClass<Transform>().lock());
if (parent.get() == 0)
{
agent_aspect->GetLog()->Error() << "(MoveAgent) ERROR: can't get parent node.\n";
return false;
}
Leaf::TLeafList leafList;
parent->ListChildrenSupportingClass<RigidBody>(leafList, true);
if (leafList.size() == 0)
{
agent_aspect->GetLog()->Error()
<< "(MoveAgent) ERROR: agent aspect doesn't have "
<< "children of type Body\n";
return false;
}
Leaf::TLeafList::iterator iter = leafList.begin();
// move all child bodies
for (; iter != leafList.end(); ++iter)
{
boost::shared_ptr<RigidBody> childBody =
shared_dynamic_cast<RigidBody>(*iter);
Vector3f childPos = childBody->GetPosition();
childBody->SetPosition(pos + (childPos-agentPos));
childBody->SetVelocity(Vector3f(0,0,0));
childBody->SetAngularVelocity(Vector3f(0,0,0));
}
return true;
}
void
InitEffector::PrePhysicsUpdateInternal(float /*deltaTime*/)
{
if ( ( mAction.get() == 0 ) ||
(mGameState.get() == 0) ||
(mAgentAspect.get() == 0)
)
{
return;
}
boost::shared_ptr<InitAction> initAction =
dynamic_pointer_cast<InitAction>(mAction);
mAction.reset();
if (initAction.get() == 0)
{
GetLog()->Error()
<< "ERROR: (InitEffector) cannot realize an unknown ActionObject\n";
return;
}
// search for the AgentState
boost::shared_ptr<AgentState> state = static_pointer_cast<AgentState>
(mAgentAspect->GetChildOfClass("AgentState", true));
if (state.get() == 0)
{
GetLog()->Error()
<< "ERROR: (InitEffector) cannot find AgentState\n";
return;
}
// register the uniform number and team index to the GameStateAspect
mGameState->RequestUniform
(state, initAction->GetName(), initAction->GetNumber());
TTeamIndex team = state->GetTeamIndex();
// request an initial position for the agent and move it there
Vector3f pos = mGameState->RequestInitPosition(team);
// request the initial orientation for the agent and turn
float angle = mGameState->RequestInitOrientation(team);
SoccerBase::MoveAndRotateAgent(mAgentAspect, pos, angle);
#if 0
Matrix mat;
mat.RotationZ(gDegToRad(angle));
// agents may be encapsulated in their own collision spaces, so we need
// to get the parent of the parent of the agent aspect in this case
boost::shared_ptr<Transform> parent = dynamic_pointer_cast<Transform>
(mAgentAspect->GetParentSupportingClass("Transform").lock());
if (parent.get() == 0)
{
GetLog()->Error()
<< "ERROR: (InitEffector) can not get parent of current agent aspect\n";
return;
}
Leaf::TLeafList leafList;
// get absolute position of the agent
Vector3f bodyPos = mAgentAspect->GetWorldTransform().Pos();
// get all the bodies below the parent of the agent aspect
parent->ListChildrenSupportingClass<Body>(leafList, true);
if (leafList.size() == 0)
{
GetLog()->Error()
<< "ERROR: (InitEffector) agent aspect doesn't have "
<< "children of type Body \n";
return;
}
Leaf::TLeafList::iterator iter = leafList.begin();
// move all the child bodies to the new relative position
for (iter;
iter != leafList.end();
++iter
)
{
boost::shared_ptr<Body> childBody =
dynamic_pointer_cast<Body>(*iter);
Vector3f childPos = childBody->GetPosition();
Matrix childR = childBody->GetRotation();
childR = mat*childR;
Vector3f newPos = pos + (mat.Rotate(childPos-bodyPos));
childBody->SetPosition(newPos);
childBody->SetRotation(childR);
childBody->SetVelocity(Vector3f(0,0,0));
childBody->SetAngularVelocity(Vector3f(0,0,0));
}
#endif
}