void GamePlayState::onLinkPlayerFactoryMsg(const LinkChangeMsg& msg) {
if (msg.change == LNK_ADDED) {
LOG_INFO("GamePlayState: Found StartingPoint");
// get the Link ref.
LinkPtr l = mPlayerFactoryRelation->getLink(msg.linkID);
StartingPointObjID = l->src();
}
}
int AtomSpace_getAtomByUUID( AtomSpace* this_ptr
, UUID uuid
, int* node_or_link
, char** type
, char** name
, UUID** out
, int* out_len)
{
Handle h(uuid);
if(!h) // Invalid UUID parameter.
return -1;
const std::string &str = classserver().getTypeName(h->getType());
*type = (char*) malloc(sizeof(char) * (str.length()+1));
if(! *type)
throw RuntimeException(TRACE_INFO,"Failed malloc.");
std::strcpy(*type, str.c_str());
NodePtr ptr = NodeCast(h);
if(ptr){ // It is a node.
*node_or_link = 1;
const std::string &str = ptr->getName();
*name = (char*) malloc(sizeof(char) * (str.length()+1));
if(! *name)
throw RuntimeException(TRACE_INFO,"Failed malloc.");
std::strcpy(*name, str.c_str());
return 0;
}else{ // It is a link.
*node_or_link = 0;
LinkPtr lnk = LinkCast(h);
if(!lnk)
throw RuntimeException(TRACE_INFO,"Error in cast Link.");
*out_len = lnk->getArity();
*out = (UUID*) malloc(sizeof(UUID) * (*out_len));
if(! *out)
throw RuntimeException(TRACE_INFO,"Failed malloc.");
int i;
for(i=0;i<(*out_len);i++)
(*out)[i]=lnk->getOutgoingAtom(i).value();
return 0;
}
}
int AtomSpace_getAtomByHandle( AtomSpace* this_ptr
, UUID handle
, char** type
, char** name
, UUID** out
, int* out_len)
{
Handle h(handle);
if(!h)
throw InvalidParamException(TRACE_INFO,
"Invalid Handler parameter.");
const std::string &str = classserver().getTypeName(h->getType());
(*type) = (char*) malloc(sizeof(char) * (str.length()+1));
if(!(*type))
throw RuntimeException(TRACE_INFO,"Failed malloc.");
std::strcpy(*type, str.c_str());
NodePtr ptr = NodeCast(h);
if(ptr){//It is a node.
const std::string &str = ptr->getName();
(*name) = (char*) malloc(sizeof(char) * (str.length()+1));
if(!(*name))
throw RuntimeException(TRACE_INFO,"Failed malloc.");
std::strcpy(*name, str.c_str());
return 1;
}else{//It is a link.
LinkPtr lnk = LinkCast(h);
if(!lnk)
throw RuntimeException(TRACE_INFO,"Error in cast Link.");
*out_len=lnk->getArity();
(*out) = (UUID*) malloc(sizeof(UUID) * (*out_len));
if(!(*out))
throw RuntimeException(TRACE_INFO,"Failed malloc.");
int i;
for(i=0;i<(*out_len);i++)
(*out)[i]=lnk->getOutgoingAtom(i).value();
return 0;
}
}
void AGXContinuousTrack::createTrackConstraint()
{
AGXLinkPtr agxFootLinkStart = _feet[0];
AGXLinkPtr agxFootLinkEnd = _feet[_feet.size() - 1];
// Connect footLinkStart and footLinkEnd with Hinge
// Create Hinge with world coordination
LinkPtr link = agxFootLinkStart->getOrgLink();
const Vector3& a = link->a(); // local
const Vector3 aw = link->attitude() * a; // world
const Vector3& p = link->p(); // world
AGXHingeDesc hd;
hd.frameAxis = agx::Vec3(aw(0), aw(1), aw(2));
hd.frameCenter = agx::Vec3(p(0), p(1), p(2));
hd.rigidBodyA = agxFootLinkStart->getAGXRigidBody();
hd.rigidBodyB = agxFootLinkEnd->getAGXRigidBody();
hd.motor.enable = false;
hd.lock.enable = false;
hd.range.enable = false;
agx::ConstraintRef constraint = AGXObjectFactory::createConstraint(hd);
link->setJointType(Link::ROTATIONAL_JOINT);
agxFootLinkStart->setAGXConstraint(constraint);
getAGXBody()->getAGXScene()->getSimulation()->add(constraint);
// Create PlaneJoint to prvent the track falling off
// Create joint with parent(example:chasis) coordination
const Vector3& b = link->b();
const agx::Vec3 a0 = agx::Vec3(a(0), a(1), a(2)); // Rotate axis of track. local
const agx::Vec3& b0 = agx::Vec3(b(0), b(1), b(2)); // Vector from parent to footLinkStart
const agx::Vec3& p0 = (a0 * b0) * a0; // Middle position of track(projection vector of b0 to a0). local
agx::Vec3 nx = agx::Vec3::Z_AXIS().cross(a0); // Check a0 is parallel to Z. X-Y consists plane joint.
agx::OrthoMatrix3x3 rotation;
if (nx.normalize() > 1.0e-6) {
nx.normal();
rotation.setColumn(0, nx);
agx::Vec3 ny = a0.cross(nx);
ny.normal();
rotation.setColumn(1, ny);
rotation.setColumn(2, a0);
}
agx::AffineMatrix4x4 af(rotation, p0);
AGXPlaneJointDesc pd;
pd.frameB = AGXObjectFactory::createFrame();
pd.frameB->setMatrix(af);
pd.rigidBodyB = agxFootLinkStart->getAGXParentLink()->getAGXRigidBody();
// Generate collision group name to disable collision between tracks
std::stringstream trackCollsionGroupName;
trackCollsionGroupName.str("");
trackCollsionGroupName << "SelfCollisionContinuousTrack" << agx::UuidGenerator().generate().str() << std::flush;
getAGXBody()->addCollisionGroupNameToDisableCollision(trackCollsionGroupName.str());
for (int i = 0; i < (int)_feet.size(); ++i) {
AGXLinkPtr agxLink = _feet[i];
// Add plane joint
pd.frameA = AGXObjectFactory::createFrame();
pd.rigidBodyA = agxLink->getAGXRigidBody();
agx::PlaneJointRef pj = AGXObjectFactory::createConstraintPlaneJoint(pd);
getAGXBody()->getAGXScene()->getSimulation()->add((agx::ConstraintRef)pj);
// Force enable collision between other links
agxLink->getAGXGeometry()->removeGroup(getAGXBody()->getCollisionGroupName());
// Disable collision between tracks
agxLink->getAGXGeometry()->addGroup(trackCollsionGroupName.str());
}
}
