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;
}
AABB2 SoccerBase::GetAgentBoundingRect(const Leaf& base)
{
AABB2 boundingRect;
boost::shared_ptr<Space> parent = base.FindParentSupportingClass<Space>().lock();
if (!parent)
{
base.GetLog()->Error()
<< "(GetAgentBoundingBox) ERROR: can't get parent node.\n";
return boundingRect;
}
/* We can't simply use the GetWorldBoundingBox of the space node, becuase
* (at least currently) it'll return a wrong answer. Currently, the space
* object is always at (0,0,0) which is encapsulated in the result of it's
* GetWorldBoundingBox method call.
*/
Leaf::TLeafList baseNodes;
parent->ListChildrenSupportingClass<Collider>(baseNodes,true);
if (baseNodes.empty())
{
base.GetLog()->Error()
<< "(GetAgentBoundingBox) ERROR: space object doesn't have any"
<< " children of type BaseNode.\n";
}
for (Leaf::TLeafList::iterator i = baseNodes.begin(); i!= baseNodes.end(); ++i)
{
boost::shared_ptr<BaseNode> node = shared_static_cast<BaseNode>(*i);
const AABB3 &box = node->GetWorldBoundingBox();
boundingRect.Encapsulate(box.minVec.x(), box.minVec.y());
boundingRect.Encapsulate(box.maxVec.x(), box.maxVec.y());
}
return boundingRect;
}
void
MonitorServer::CollectItemPredicates(bool initial, PredicateList& pList)
{
Leaf::TLeafList itemList;
ListChildrenSupportingClass<MonitorItem>(itemList);
for (
Leaf::TLeafList::const_iterator iter = itemList.begin();
iter != itemList.end();
++iter
)
{
boost::shared_ptr<MonitorItem> item =
shared_static_cast<MonitorItem>(*iter);
if (initial)
{
item->GetInitialPredicates(pList);
} else
{
item->GetPredicates(pList);
}
}
}
void kinematicFrame::UpdateCached()
{
mJoints.clear();
wxSizer* sizer = mCtrScrollWnd->GetSizer();
if (sizer == 0)
{
assert(false);
return;
}
shared_ptr<SimSpark> spark = wxGetApp().GetSpark();
if (spark.get() == 0)
{
return;
}
mParent.Update(spark->GetCore());
shared_ptr<Node> node = shared_dynamic_cast<Node>(mParent.get());
if (node.get() == 0)
{
return;
}
mCtrScrollWnd->Freeze();
bool delete_windows = true;
sizer->Clear(delete_windows);
Leaf::TLeafList joints;
bool recursive = true;
node->ListChildrenSupportingClass<Joint>(joints,recursive);
for (
Leaf::TLeafList::iterator iter = joints.begin();
iter != joints.end();
++iter
)
{
shared_ptr<Joint> joint = shared_static_cast<Joint>(*iter);
int jt = joint->GetType();
switch (jt)
{
case dJointTypeFixed:
break;
case dJointTypeUniversal:
AddJointDescription(joint);
AddJointControl(joint, 0);
AddJointControl(joint, 1);
break;
default:
AddJointDescription(joint);
AddJointControl(joint, 0);
break;
}
}
mCtrScrollWnd->Thaw();
mCtrScrollWnd->Layout();
}
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
}
Leaf::TLeafList::const_iterator Leaf::end() const
{
return gFakeChildren.end();
}
Leaf::TLeafList::iterator Leaf::end()
{
return gFakeChildren.end();
}
Leaf::TLeafList::const_iterator Leaf::begin() const
{
return gFakeChildren.begin();
}
Leaf::TLeafList::iterator Leaf::begin()
{
return gFakeChildren.begin();
}
