TEST(DcdCollisionTest, RigidRigidCollisionTest)
{
auto sphere1 = std::make_shared<Blocks::SphereElement>("Sphere1");
auto sphere2 = std::make_shared<Blocks::SphereElement>("Sphere2");
sphere2->setPose(Math::makeRigidTransform(Math::Quaterniond::Identity(), Vector3d(0.0, 0.0, 0.5)));
auto runtime = std::make_shared<Framework::Runtime>();
auto scene = runtime->getScene();
scene->addSceneElement(sphere1);
scene->addSceneElement(sphere2);
std::vector<std::shared_ptr<Physics::Representation>> physicsRepresentations;
physicsRepresentations.push_back(sphere1->getComponents<Physics::Representation>()[0]);
physicsRepresentations.push_back(sphere2->getComponents<Physics::Representation>()[0]);
auto sphere1Collision = sphere1->getComponents<Collision::Representation>()[0];
auto sphere2Collision = sphere2->getComponents<Collision::Representation>()[0];
sphere1Collision->update(0.0);
sphere2Collision->update(0.0);
std::vector<std::shared_ptr<Collision::Representation>> collisionRepresentations;
collisionRepresentations.push_back(sphere1Collision);
collisionRepresentations.push_back(sphere2Collision);
auto prepareState = std::make_shared<PrepareCollisionPairs>(false);
auto stateTmp = std::make_shared<PhysicsManagerState>();
stateTmp->setRepresentations(physicsRepresentations);
stateTmp->setCollisionRepresentations(collisionRepresentations);
auto state = prepareState->update(0.0, stateTmp); // This step prepares the collision pairs
ASSERT_EQ(1u, state->getCollisionPairs().size());
auto dcdCollision = std::make_shared<DcdCollision>(false);
std::shared_ptr<PhysicsManagerState> newState = dcdCollision->update(1.0, state);
EXPECT_TRUE(newState->getCollisionPairs().at(0)->hasContacts());
}
void SkeletalCreature::finishInit() {
Agent::finishInit();
processGenes();
skeletonInit();
setPose(0);
}
void Skeleton::setPose(const vector<double>& state, bool bCalcTrans, bool bCalcDeriv) {
VectorXd x(state.size());
for (unsigned int i = 0; i < state.size(); i++) {
x(i) = state[i];
}
setPose(x, bCalcTrans, bCalcDeriv);
}
// Set and publish pose.
void Publisher::publishStateAsCallback(
const okvis::Time & t, const okvis::kinematics::Transformation & T_WS)
{
setTime(t);
setPose(T_WS); // TODO: provide setters for this hack
publishPose();
}
void StaticEntity::setCubeCoordinate(const CoordI& cc)
{
cube_coordinate_ = cc;
Vec3f pos = Properties::WorldToPosition(cube_coordinate_);
setPose(pos);
Vec3f center_pos = Properties::WorldToPositionCenter(cube_coordinate_);
setCenterPosition(center_pos);
}
void Object::setPoseLWH(Pose pose, Point bounding_box_lwh)
{
bounding_box_lwh_ = bounding_box_lwh;
scale_.x = bounding_box_lwh_.x;
scale_.y = bounding_box_lwh_.y;
scale_.z = bounding_box_lwh_.z;
setPose(pose);
}
void SkeletalCreature::setPoseGene(unsigned int poseno) {
std::map<unsigned int, creaturePoseGene *>::iterator i = posegenes.find(poseno);
if (i == posegenes.end()) return; // TODO: is there a better behaviour here?
creaturePoseGene *g = i->second;
assert(g->poseno == poseno);
gaitgene = 0;
walking = false; // TODO: doesn't belong here, does it? really the idea of a 'walking' bool is horrid
setPose(g->getPoseString());
}
void
MeshPose::eventReceived(const std::string &sender, const std::string &eventType, curitiba::event_::IEventData *evt)
{
if(eventType=="NEXT_POSE") {
m_ActivePose++;
if (m_ActivePose >= m_vOffsets.size())
m_ActivePose = 0;
setPose(m_ActivePose);
}
}
TEST_F(OsgSkeletonRepresentationRenderTests, WithNeutralPoseTest)
{
auto graphics = std::make_shared<SurgSim::Graphics::OsgSkeletonRepresentation>("Skeleton");
graphics->loadModel("OsgSkeletonRepresentationRenderTests/rigged_cylinder.osgt");
graphics->setSkinningShaderFileName("Shaders/skinning.vert");
graphics->setNeutralBonePose("Bone",
makeRigidTransform(makeRotationQuaternion(M_PI_4, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero()));
auto sceneElement = std::make_shared<SurgSim::Framework::BasicSceneElement>("Rigged Cylinder");
sceneElement->addComponent(graphics);
scene->addSceneElement(sceneElement);
viewElement->setPose(SurgSim::Math::makeRigidTransform(
Vector3d(-0.3, 0.3, -0.3),
Vector3d(0.0, 0.0, 0.0),
Vector3d(0.0, 0.0, 1.0)));
runtime->start();
boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
std::pair<RigidTransform3d, RigidTransform3d> rootTransform;
rootTransform.first =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
rootTransform.second =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 1.0, 1.0)), Vector3d::Zero());
std::pair<RigidTransform3d, RigidTransform3d> boneTransform;
boneTransform.first =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d::Zero());
boneTransform.second =
makeRigidTransform(makeRotationQuaternion(0.0, Vector3d(1.0, 0.0, 0.0)), Vector3d(0.0, 4.0, 0.0));
int numSteps = 1000;
for (int i = 0; i < numSteps; ++i)
{
double t = static_cast<double>(i) / numSteps;
RigidTransform3d pose = interpolate(rootTransform, t);
sceneElement->setPose(pose);
pose = interpolate(boneTransform, t / 3.0);
graphics->setBonePose("Bone", RigidTransform3d::Identity());
graphics->setBonePose("Bone_001", pose);
graphics->setBonePose("Bone_002", pose);
graphics->setBonePose("Bone_003", pose);
/// The total number of steps should complete in 5 seconds
boost::this_thread::sleep(boost::posix_time::milliseconds(5000 / numSteps));
}
runtime->stop();
}
void rosSpin() {
double dt;
Eigen::Vector3d t(0.0, 0.0, 0.0);
Eigen::Vector3d t_new(0.0, 0.0, 0.0);
Eigen::Vector3d t_vel(0.0, 0.0, 0.0);
Eigen::Vector3d t_imu(0.0, 0.0, 0.0);
Eigen::Vector3d t_arm(-0.19, 0.0, 0.02);
Eigen::Quaterniond q_imu;
ros::Rate rate(15);
ros::Time time_prev, time_now;
time_prev = ros::Time::now();
while(ros::ok()) {
ros::spinOnce();
time_now = ros::Time::now();
dt = (time_now - time_prev).toSec();
time_prev = time_now;
// Compute Rotation
q_imu = w_imu.getQuaternionForAngularChange(dt);
q = q * q_imu;
// Compute Translation
t_imu = v_wheel.getRotationArm()*v_wheel.getTranslation() - q_imu.toRotationMatrix()*t_arm + t_arm;
t_new = t + q.toRotationMatrix()*t_imu;
t_vel = (t_new - t)/dt;
t = t_new;
setPose(t, q, t_new);
pub_data.publish(odometry);
rate.sleep();
}
};
void CViewerContainer::showRangeScan(CNode* node)
{
CRangeScanNode* obsNode = dynamic_cast<CRangeScanNode*>(node);
ASSERT_(obsNode);
auto obj = mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
obj->setScan(*(obsNode->observation().get()));
obj->setPose(obsNode->getPose());
obj->setSurfaceColor(1.0f, 0.0f, 0.0f, 0.5f);
forEachGl([obsNode, obj](CGlWidget* gl) {
gl->setSelected(obsNode->getPose().asTPose());
gl->setLaserScan(obj);
});
}
void
MeshPose::setPose(std::string aName)
{
unsigned int index = 0;
while (index < m_vNames.size() && m_vNames[index] != aName) {
index++;
}
if (index < m_vNames.size()) {
m_ActivePose = index;
setPose(index);
}
resetCompilationFlags();
}
bool Primitive::restore(FILE* f){
Pose pose;
Pos vel;
Pos avel;
if ( fread ( pose.ptr() , sizeof ( Pose::value_type), 16, f ) == 16 )
if( fread ( vel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 )
if( fread ( avel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 ){
setPose(pose);
if(body){
dBodySetLinearVel(body,vel.x(),vel.y(),vel.z());
dBodySetAngularVel(body,avel.x(),avel.y(),avel.z());
}
return true;
}
fprintf ( stderr, "Primitve::restore: cannot read primitive from data\n" );
return false;
}
void cv::viz::Widget3D::updatePose(const Affine3d &pose)
{
vtkProp3D *actor = vtkProp3D::SafeDownCast(WidgetAccessor::getProp(*this));
CV_Assert("Widget is not 3D." && actor);
vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
if (!matrix)
{
setPose(pose);
return;
}
Affine3d updated_pose = pose * Affine3d(*matrix->Element);
matrix = vtkmatrix(updated_pose.matrix);
actor->SetUserMatrix(matrix);
actor->Modified();
}
void SkeletalCreature::gaitTick() {
if (!gaitgene) return;
uint8 pose = gaitgene->pose[gaiti];
if (pose == 0) {
if (gaiti == 0) return; // non-worky gait
gaiti = 0;
gaitTick();
return;
}
std::map<unsigned int, creaturePoseGene *>::iterator i = posegenes.find(pose);
if (i != posegenes.end()) {
creaturePoseGene *poseg = i->second;
assert(poseg->poseno == pose);
setPose(poseg->getPoseString());
}
gaiti++; if (gaiti > 7) gaiti = 0;
}
//------------------------------------------------------------------------------
//!
RCP<SkeletalAnimation>
Puppeteer::retarget(
Skeleton* srcSkel,
Skeleton* dstSkel,
SkeletalAnimation* srcAnim,
bool constraints,
const Vec3f& userScale
)
{
DBG_BLOCK( os_pup, "Puppeteer::retarget(" << srcSkel << ", " << dstSkel << ", " << srcAnim << ")" );
if( (srcSkel == 0) || (dstSkel == 0) || (srcAnim == 0) )
{
// Safeguarding.
return NULL;
}
// Compute scaling factors.
Vec3f srcRoot;
Vec3f dstRoot;
float srcH, srcW;
float dstH, dstW;
getRootHeightWidth( srcSkel, srcRoot, srcH, srcW );
getRootHeightWidth( dstSkel, dstRoot, dstH, dstW );
float rootScaleY = dstH / srcH;
float stepScale = dstW / srcW;
// Detect step constraints. (might need extra parameters to help this stage)
// 1. Thresholds
Vector< ContactList > contactLists;
float maxVelDetThreshold = 0.1f * srcH / srcAnim->rate();
float maxPosExtThreshold = 0.05f * srcH;
uint minNumFramesThreshold = 5;
// 2. Bones candidates.
Vector<int> boneCandidates;
if( constraints )
{
for( uint i = 0; i < srcSkel->numLimbs(); ++i )
{
uint nID = srcSkel->limb(i).nodeID();
Skeleton::DepthFirstIterator it = srcSkel->depthFirst( nID );
for( uint j = 0; j < 3 && it(); ++j, ++it )
{
nID = *it;
}
boneCandidates.pushBack( srcSkel->node(nID).boneID() );
}
}
// 3. Contacts.
findContactCandidates(
srcSkel, srcAnim,
maxVelDetThreshold,
maxPosExtThreshold,
minNumFramesThreshold,
boneCandidates,
contactLists
);
// Set a default (constant) influence radius for all contact candidates.
float influenceRadius = srcH * 0.3f;
for( uint cl = 0; cl < contactLists.size(); ++cl )
{
ContactList& contactList = contactLists[cl];
contactList.influenceRadius( influenceRadius );
}
// Retarget animation.
// 1. Clone animation.
RCP<SkeletalAnimation> dstAnim = srcAnim->clone();
dstAnim->skeleton( dstSkel );
// 2. Rescale root based on lower body height and readjust bone angle
// to take into account the difference in the canonical pose.
// Compute transformation from the src pose to the dst pose.
Vector<Quatf> convert(srcSkel->numBones());
for( uint b = 0; b < srcSkel->numBones(); ++b )
{
convert[b] = dstSkel->bone(b).orientation() * srcSkel->bone(b).orientation().getInversed();
}
Vec3f floorHeight( 0.0f, computeFloorHeight( srcSkel, srcAnim ), 0.0f );
Vec3f scale = userScale * Vec3f( stepScale, rootScaleY, rootScaleY );
for( uint p = 0; p < dstAnim->numPoses(); ++p )
{
// Adjust root.
SkeletalPose& pose = *dstAnim->pose(p);
Mat4f mat = pose.orientation().toMatrix();
Vec3f goalRoot = ( pose.position() + mat*srcRoot - floorHeight )*scale;
Vec3f curRoot = mat * dstRoot;
pose.position( goalRoot-curRoot );
// Adjust bones angles.
for( uint b = 0; b < pose.bones().size(); ++b )
{
pose.bones()[b] = convert[b]*pose.bones()[b];
}
}
// 3. Compute targets constraints.
for( uint cl = 0; cl < contactLists.size(); ++cl )
{
ContactList& contactList = contactLists[cl];
for( uint c = 0; c < contactList.numContacts(); ++c )
{
Contact& contact = contactList.contact(c);
contact.target( (contact.target() - floorHeight)*scale );
}
}
// 4. Retarget each frame of animation by imposing constraints on limbs
// positions.
Skeleton::Instance srcInst;
srcInst.skeleton( srcSkel );
Skeleton::Instance dstInst;
dstInst.skeleton( dstSkel );
ConstraintConverter cc( srcInst, contactLists );
for( uint p = 0; p < dstAnim->numPoses(); ++p )
{
setPose( srcInst, srcAnim, p ); // The ConstraintConverter needs the proper positions.
setPose( dstInst, dstAnim.ptr(), p );
resolveConstraints( dstInst, cc.computePoseConstraints(p) );
dstAnim->setPose( p, dstInst.pose(), dstInst.offset() );
}
// 5. Scale animation speed based on relative scales.
dstAnim->velocity( dstAnim->velocity() * scale );
return dstAnim;
}
// Should show two rotating cubes, one in the middle of the screen being rendered normally, the
// other one in the top right hand corner, being rendered onto a texture mapped on a quad
TEST_F(OsgScreenSpaceQuadRenderTests, RenderTextureTest)
{
auto defaultCamera = viewElement->getCamera();
auto camera = std::make_shared<OsgCamera>("RenderPass");
camera->setProjectionMatrix(defaultCamera->getProjectionMatrix());
camera->setRenderGroupReference("RenderPass");
camera->setGroupReference(SurgSim::Graphics::Representation::DefaultGroupName);
auto dimensions = viewElement->getView()->getDimensions();
std::shared_ptr<OsgRenderTarget2d> renderTargetOsg =
std::make_shared<OsgRenderTarget2d>(dimensions[0], dimensions[1], 1.0, 2, true);
camera->setRenderTarget(renderTargetOsg);
viewElement->addComponent(camera);
int screenWidth = dimensions[0];
int screenHeight = dimensions[1];
int width = dimensions[0] / 3;
int height = dimensions[1] / 3;
std::shared_ptr<ScreenSpaceQuadRepresentation> quad;
quad = makeQuad("Color1", width, height, screenWidth - width, screenHeight - height);
quad->setTexture(renderTargetOsg->getColorTargetOsg(0));
viewElement->addComponent(quad);
quad = makeQuad("Color2", width, height, screenWidth - width, screenHeight - height * 2);
quad->setTexture(renderTargetOsg->getColorTargetOsg(1));
viewElement->addComponent(quad);
quad = makeQuad("Depth", width, height, 0.0, screenHeight - height);
quad->setTexture(renderTargetOsg->getDepthTargetOsg());
viewElement->addComponent(quad);
Quaterniond quat = Quaterniond::Identity();
RigidTransform3d startPose = SurgSim::Math::makeRigidTransform(quat, Vector3d(0.0, 0.0, -0.2));
quat = SurgSim::Math::makeRotationQuaternion(M_PI, Vector3d::UnitY().eval());
RigidTransform3d endPose = SurgSim::Math::makeRigidTransform(quat, Vector3d(0.0, 0.0, -0.2));
auto box = std::make_shared<OsgBoxRepresentation>("Graphics");
box->setSizeXYZ(0.05, 0.05, 0.05);
box->setGroupReference("RenderPass");
auto boxElement1 = std::make_shared<BasicSceneElement>("Box 1");
boxElement1->addComponent(box);
boxElement1->setPose(startPose);
scene->addSceneElement(boxElement1);
box = std::make_shared<OsgBoxRepresentation>("Graphics");
box->setSizeXYZ(0.05, 0.05, 0.05);
auto boxElement2 = std::make_shared<BasicSceneElement>("Box 2");
boxElement2->addComponent(box);
boxElement2->setPose(startPose);
scene->addSceneElement(boxElement2);
/// Run the thread
runtime->start();
int numSteps = 100;
boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
for (int i = 0; i < numSteps; ++i)
{
double t = static_cast<double>(i) / numSteps;
boxElement1->setPose(SurgSim::Testing::interpolate<RigidTransform3d>(startPose, endPose, t));
boxElement2->setPose(SurgSim::Testing::interpolate<RigidTransform3d>(endPose, startPose, t));
boost::this_thread::sleep(boost::posix_time::milliseconds(1000 / 100));
}
graphicsManager->dumpDebugInfo();
}
void phx::core::scene::local::step(neb::core::TimeStep const & ts) {
BOOST_LOG_CHANNEL_SEV(lg, "phx core scene", debug) << __PRETTY_FUNCTION__ << " dt = " << ts.dt;
neb::core::scene::local::step(ts);
phx::core::scene::base::step(ts);
auto app = neb::app::base::global();
// timer
//timer_set_.step(time);
//physx::PxU32 nbPxactor = px_scene_->getNbActors(physx::PxActorTypeSelectionFlag::eRIGID_DYNAMIC);
// PxScene
assert(px_scene_ != NULL);
px_scene_->simulate(ts.dt);
px_scene_->fetchResults(true);
// retrieve array of actors that moved
physx::PxU32 nb_active_transforms;
const physx::PxActiveTransform* active_transforms = px_scene_->getActiveTransforms(nb_active_transforms);
BOOST_LOG_CHANNEL_SEV(lg, "phx core scene", debug)
<< "active transforms: " << nb_active_transforms;
//physx::PxTransform pose;
physx::PxTransform pose;
// update each render object with the new transform
for(physx::PxU32 i = 0; i < nb_active_transforms; ++i) {
//physx::PxActor* px_actor = active_transforms[i].actor;
//printf( "actor type = %i\n", px_actor->getType() );
physx::PxActor* pxactor = active_transforms[i].actor;
assert(pxactor);
void* ud = active_transforms[i].userData;
assert(ud);
physx::PxRigidBody* pxrigidbody = pxactor->isRigidBody();
neb::core::actor::base* pactor = static_cast<neb::core::actor::base*>(ud);
auto actor = pactor->isActorBase();
assert(actor);
if(actor) {
pose = active_transforms[i].actor2World;
actor->setPose(neb::core::pose(
phx::util::convert(pose.p),
phx::util::convert(pose.q)
));
BOOST_LOG_CHANNEL_SEV(lg, "phx core scene", debug)
<< std::setw(8) << "p"
<< std::setw(8) << pose.p.x
<< std::setw(8) << pose.p.y
<< std::setw(8) << pose.p.z;
if(pxrigidbody != NULL) {
auto rigidbody = actor->isActorRigidBody();
if(!rigidbody) {
std::cout << typeid(*actor).name() << std::endl;
abort();
}
physx::PxVec3 v(pxrigidbody->getLinearVelocity());
rigidbody->velocity_ = phx::util::convert(v);
//v.print();
}
actor->flag_.set(neb::core::actor::util::Flag::E::SHOULD_UPDATE);
}
}
// vehicle
//physx::PxVec3 g(0,-0.25,0);
//vehicle_manager_.vehicle_suspension_raycasts(px_scene_);
//vehicle_manager_.update((float)dt, g);
send_actor_update();
}
template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
int n = static_cast<int> (getNumVertices ());
if (n < 2)
{
PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
return;
}
for (int i = 0; i < max_iterations_; ++i)
{
// Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
typename SLAMGraph::edge_iterator e, e_end;
for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
computeEdge (*e);
// Declare matrices G and B
Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));
// Start at 1 because 0 is the reference pose
for (int vi = 1; vi != n; ++vi)
{
for (int vj = 0; vj != n; ++vj)
{
// Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
Edge e;
bool present1, present2;
boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
if (!present1)
{
boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
if (!present2)
continue;
}
// Fill in elements of G and B
if (vj > 0)
G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
}
}
// Computation of the linear equation system: GX = B
// TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
// Update the poses
float sum = 0.0;
for (int vi = 1; vi != n; ++vi)
{
Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
sum += difference_pose.norm ();
setPose (vi, getPose (vi) + difference_pose);
}
// Convergence check
if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
return;
}
}
/**
* This method calculates the moves for the AI player
*
* @param movePtr Pointer to move the move list into
*/
bool AIPlayer::getAIMove(Move *movePtr)
{
*movePtr = NullMove;
// Use the eye as the current position.
MatrixF eye;
getEyeTransform(&eye);
Point3F location = eye.getPosition();
Point3F rotation = getRotation();
#ifdef TORQUE_NAVIGATION_ENABLED
if(mDamageState == Enabled)
{
if(mMoveState != ModeStop)
updateNavMesh();
if(!mFollowData.object.isNull())
{
if(mPathData.path.isNull())
{
if((getPosition() - mFollowData.object->getPosition()).len() > mFollowData.radius)
followObject(mFollowData.object, mFollowData.radius);
}
else
{
if((mPathData.path->mTo - mFollowData.object->getPosition()).len() > mFollowData.radius)
repath();
else if((getPosition() - mFollowData.object->getPosition()).len() < mFollowData.radius)
{
clearPath();
mMoveState = ModeStop;
throwCallback("onTargetInRange");
}
else if((getPosition() - mFollowData.object->getPosition()).len() < mAttackRadius)
{
throwCallback("onTargetInFiringRange");
}
}
}
}
#endif // TORQUE_NAVIGATION_ENABLED
// Orient towards the aim point, aim object, or towards
// our destination.
if (mAimObject || mAimLocationSet || mMoveState != ModeStop)
{
// Update the aim position if we're aiming for an object
if (mAimObject)
mAimLocation = mAimObject->getPosition() + mAimOffset;
else
if (!mAimLocationSet)
mAimLocation = mMoveDestination;
F32 xDiff = mAimLocation.x - location.x;
F32 yDiff = mAimLocation.y - location.y;
if (!mIsZero(xDiff) || !mIsZero(yDiff))
{
// First do Yaw
// use the cur yaw between -Pi and Pi
F32 curYaw = rotation.z;
while (curYaw > M_2PI_F)
curYaw -= M_2PI_F;
while (curYaw < -M_2PI_F)
curYaw += M_2PI_F;
// find the yaw offset
F32 newYaw = mAtan2( xDiff, yDiff );
F32 yawDiff = newYaw - curYaw;
// make it between 0 and 2PI
if( yawDiff < 0.0f )
yawDiff += M_2PI_F;
else if( yawDiff >= M_2PI_F )
yawDiff -= M_2PI_F;
// now make sure we take the short way around the circle
if( yawDiff > M_PI_F )
yawDiff -= M_2PI_F;
else if( yawDiff < -M_PI_F )
yawDiff += M_2PI_F;
movePtr->yaw = yawDiff;
// Next do pitch.
if (!mAimObject && !mAimLocationSet)
{
// Level out if were just looking at our next way point.
Point3F headRotation = getHeadRotation();
movePtr->pitch = -headRotation.x;
}
else
{
// This should be adjusted to run from the
// eye point to the object's center position. Though this
// works well enough for now.
F32 vertDist = mAimLocation.z - location.z;
F32 horzDist = mSqrt(xDiff * xDiff + yDiff * yDiff);
F32 newPitch = mAtan2( horzDist, vertDist ) - ( M_PI_F / 2.0f );
if (mFabs(newPitch) > 0.01f)
{
Point3F headRotation = getHeadRotation();
movePtr->pitch = newPitch - headRotation.x;
}
}
}
}
else
{
// Level out if we're not doing anything else
Point3F headRotation = getHeadRotation();
movePtr->pitch = -headRotation.x;
}
// Move towards the destination
if (mMoveState != ModeStop)
{
F32 xDiff = mMoveDestination.x - location.x;
F32 yDiff = mMoveDestination.y - location.y;
// Check if we should mMove, or if we are 'close enough'
if (mFabs(xDiff) < mMoveTolerance && mFabs(yDiff) < mMoveTolerance)
{
mMoveState = ModeStop;
onReachDestination();
}
else
{
// Build move direction in world space
if (mIsZero(xDiff))
movePtr->y = (location.y > mMoveDestination.y) ? -1.0f : 1.0f;
else
if (mIsZero(yDiff))
movePtr->x = (location.x > mMoveDestination.x) ? -1.0f : 1.0f;
else
if (mFabs(xDiff) > mFabs(yDiff))
{
F32 value = mFabs(yDiff / xDiff);
movePtr->y = (location.y > mMoveDestination.y) ? -value : value;
movePtr->x = (location.x > mMoveDestination.x) ? -1.0f : 1.0f;
}
else
{
F32 value = mFabs(xDiff / yDiff);
movePtr->x = (location.x > mMoveDestination.x) ? -value : value;
movePtr->y = (location.y > mMoveDestination.y) ? -1.0f : 1.0f;
}
// Rotate the move into object space (this really only needs
// a 2D matrix)
Point3F newMove;
MatrixF moveMatrix;
moveMatrix.set(EulerF(0.0f, 0.0f, -(rotation.z + movePtr->yaw)));
moveMatrix.mulV( Point3F( movePtr->x, movePtr->y, 0.0f ), &newMove );
movePtr->x = newMove.x;
movePtr->y = newMove.y;
// Set movement speed. We'll slow down once we get close
// to try and stop on the spot...
if (mMoveSlowdown)
{
F32 speed = mMoveSpeed;
F32 dist = mSqrt(xDiff*xDiff + yDiff*yDiff);
F32 maxDist = mMoveTolerance*2;
if (dist < maxDist)
speed *= dist / maxDist;
movePtr->x *= speed;
movePtr->y *= speed;
mMoveState = ModeSlowing;
}
else
{
movePtr->x *= mMoveSpeed;
movePtr->y *= mMoveSpeed;
mMoveState = ModeMove;
}
if (mMoveStuckTestCountdown > 0)
--mMoveStuckTestCountdown;
else
{
// We should check to see if we are stuck...
F32 locationDelta = (location - mLastLocation).len();
if (locationDelta < mMoveStuckTolerance && mDamageState == Enabled)
{
// If we are slowing down, then it's likely that our location delta will be less than
// our move stuck tolerance. Because we can be both slowing and stuck
// we should TRY to check if we've moved. This could use better detection.
if ( mMoveState != ModeSlowing || locationDelta == 0 )
{
mMoveState = ModeStuck;
onStuck();
}
}
}
}
}
// Test for target location in sight if it's an object. The LOS is
// run from the eye position to the center of the object's bounding,
// which is not very accurate.
if (mAimObject)
{
if (checkInLos(mAimObject.getPointer()))
{
if (!mTargetInLOS)
{
throwCallback( "onTargetEnterLOS" );
mTargetInLOS = true;
}
}
else if (mTargetInLOS)
{
throwCallback( "onTargetExitLOS" );
mTargetInLOS = false;
}
}
Pose desiredPose = mPose;
if ( mSwimming )
desiredPose = SwimPose;
else if ( mAiPose == 1 && canCrouch() )
desiredPose = CrouchPose;
else if ( mAiPose == 2 && canProne() )
desiredPose = PronePose;
else if ( mAiPose == 3 && canSprint() )
desiredPose = SprintPose;
else if ( canStand() )
desiredPose = StandPose;
setPose( desiredPose );
// Replicate the trigger state into the move so that
// triggers can be controlled from scripts.
for( U32 i = 0; i < MaxTriggerKeys; i++ )
movePtr->trigger[ i ] = getImageTriggerState( i );
#ifdef TORQUE_NAVIGATION_ENABLED
if(mJump == Now)
{
movePtr->trigger[2] = true;
mJump = None;
}
else if(mJump == Ledge)
{
// If we're not touching the ground, jump!
RayInfo info;
if(!getContainer()->castRay(getPosition(), getPosition() - Point3F(0, 0, 0.4f), StaticShapeObjectType, &info))
{
movePtr->trigger[2] = true;
mJump = None;
}
}
#endif // TORQUE_NAVIGATION_ENABLED
mLastLocation = location;
return true;
}
