void StandardLocationService::locationUpdate(UUID source, void* buffer, uint32 length) {
Sirikata::Protocol::Loc::Container loc_container;
bool parse_success = loc_container.ParseFromString( String((char*) buffer, length) );
if (!parse_success) {
LOG_INVALID_MESSAGE_BUFFER(standardloc, error, ((char*)buffer), length);
return;
}
if (loc_container.has_update_request()) {
Sirikata::Protocol::Loc::LocationUpdateRequest request = loc_container.update_request();
TrackingType obj_type = type(source);
if (obj_type == Local) {
LocationMap::iterator loc_it = mLocations.find( source );
assert(loc_it != mLocations.end());
if (request.has_location()) {
TimedMotionVector3f newloc(
request.location().t(),
MotionVector3f( request.location().position(), request.location().velocity() )
);
loc_it->second.location = newloc;
notifyLocalLocationUpdated( source, loc_it->second.aggregate, newloc );
CONTEXT_SPACETRACE(serverLoc, mContext->id(), mContext->id(), source, newloc );
}
if (request.has_bounds()) {
BoundingSphere3f newbounds = request.bounds();
loc_it->second.bounds = newbounds;
notifyLocalBoundsUpdated( source, loc_it->second.aggregate, newbounds );
}
if (request.has_mesh()) {
String newmesh = request.mesh();
loc_it->second.mesh = newmesh;
notifyLocalMeshUpdated( source, loc_it->second.aggregate, newmesh );
}
if (request.has_orientation()) {
TimedMotionQuaternion neworient(
request.orientation().t(),
MotionQuaternion( request.orientation().position(), request.orientation().velocity() )
);
loc_it->second.orientation = neworient;
notifyLocalOrientationUpdated( source, loc_it->second.aggregate, neworient );
}
if (request.has_physics()) {
String newphy = request.physics();
loc_it->second.physics = newphy;
notifyLocalPhysicsUpdated( source, loc_it->second.aggregate, newphy );
}
}
else {
// Warn about update to non-local object
}
}
}
void SimpleCameraObjectScript::moveAction(Vector3f dir, float amount)
{
// Get the updated position
Time now = context()->simTime();
Location loc = mSelfProxy->extrapolateLocation(now);
const Quaternion &orient = loc.getOrientation();
// Request updates from spcae
TimedMotionVector3f newloc(now, MotionVector3f(Vector3f(loc.getPosition()), (orient * dir) * amount * WORLD_SCALE * .5) );
mParent->requestLocationUpdate(mID.space(), mID.object(), newloc);
}
void StandardLocationService::receiveMessage(Message* msg) {
assert(msg->dest_port() == SERVER_PORT_LOCATION);
Sirikata::Protocol::Loc::BulkLocationUpdate contents;
bool parsed = parsePBJMessage(&contents, msg->payload());
if (parsed) {
for(int32 idx = 0; idx < contents.update_size(); idx++) {
Sirikata::Protocol::Loc::LocationUpdate update = contents.update(idx);
// Its possible we'll get an out of date update. We only use this update
// if (a) we have this object marked as a replica object and (b) we don't
// have this object marked as a local object
if (type(update.object()) != Replica)
continue;
LocationMap::iterator loc_it = mLocations.find( update.object() );
// We can safely make this assertion right now because space server
// to space server loc and prox are on the same reliable channel. If
// this goes away then a) we can't make this assertion and b) we
// need an OrphanLocUpdateManager to save updates where this
// condition is false so they can be applied once the prox update
// arrives.
assert(loc_it != mLocations.end());
if (update.has_location()) {
TimedMotionVector3f newloc(
update.location().t(),
MotionVector3f( update.location().position(), update.location().velocity() )
);
loc_it->second.location = newloc;
notifyReplicaLocationUpdated( update.object(), newloc );
CONTEXT_SPACETRACE(serverLoc, msg->source_server(), mContext->id(), update.object(), newloc );
}
if (update.has_orientation()) {
TimedMotionQuaternion neworient(
update.orientation().t(),
MotionQuaternion( update.orientation().position(), update.orientation().velocity() )
);
loc_it->second.orientation = neworient;
notifyReplicaOrientationUpdated( update.object(), neworient );
}
if (update.has_bounds()) {
BoundingSphere3f newbounds = update.bounds();
loc_it->second.bounds = newbounds;
notifyReplicaBoundsUpdated( update.object(), newbounds );
}
if (update.has_mesh()) {
String newmesh = update.mesh();
loc_it->second.mesh = newmesh;
notifyReplicaMeshUpdated( update.object(), newmesh );
}
if (update.has_physics()) {
String newphy = update.physics();
loc_it->second.physics = newphy;
notifyReplicaPhysicsUpdated( update.object(), newphy );
}
}
}
delete msg;
}
int dtq::compProp(void)
{
// need myh to proceed
if (! haveMyh) return 1;
// fundamental size
ylen = 2*bigm+1;
// start computing the propagator!
prop = arma::sp_mat(ylen,ylen);
// create yvec
yvec = arma::zeros(ylen);
for (int i=-bigm; i<=bigm; i++)
yvec(bigm+i) = i*myk;
// apply f and g to yvec
fy = arma::zeros(ylen);
gy = arma::zeros(ylen);
for (int i=-bigm; i<=bigm; i++)
{
fy(bigm+i) = (*f)(yvec(bigm+i),curtheta);
gy(bigm+i) = (*g)(yvec(bigm+i),curtheta);
}
// normalization "constant"
arma::vec c0mod = 1.0/(sqrt(2.0*(arma::datum::pi)*myh)*gy);
// variance
arma::vec gy2 = gy % gy;
arma::vec myvar = gy2*myh;
// compute and set main diagonal
// prop.diag() = maindiag;
arma::vec propvals = arma::exp(-(myh/2.0)*(fy%fy)/gy2) % c0mod;
arma::umat proploc(2, ylen);
proploc.row(0) = arma::regspace<arma::urowvec>(0, (ylen-1));
proploc.row(1) = arma::regspace<arma::urowvec>(0, (ylen-1));
// superdiagonals
bool done = false;
int curdiag = 1;
double mytol = 2.0e-16;
double refsum = arma::sum(arma::abs(propvals))*myk;
while (! done)
{
arma::vec mymean = curdiag*myk + fy*myh;
arma::vec thisdiag = arma::exp(-mymean%mymean/(2.0*myvar))%c0mod;
thisdiag = thisdiag.tail(ylen - curdiag);
double thissum = arma::sum(arma::abs(thisdiag));
if ((curdiag == 1) || (thissum > mytol*refsum))
{
// prop.diag(curdiag) = thisdiag;
arma::umat newloc(2, ylen-curdiag);
newloc.row(0) = arma::regspace<arma::urowvec>(0, (ylen-curdiag-1));
newloc.row(1) = newloc.row(0) + curdiag;
proploc = join_horiz(proploc, newloc);
propvals = join_vert(propvals, thisdiag);
curdiag++;
if (curdiag == ylen) done = true;
}
else done = true;
}
int maxdiag = curdiag;
for (curdiag=1; curdiag<=maxdiag; curdiag++)
{
arma::vec mymean = -curdiag*myk + fy*myh;
arma::vec thisdiag = arma::exp(-mymean%mymean/(2.0*myvar))%c0mod;
thisdiag = thisdiag.head(ylen - curdiag);
// prop.diag(-curdiag) = thisdiag;
arma::umat newloc(2, ylen-curdiag);
newloc.row(1) = arma::regspace<arma::urowvec>(0, (ylen-curdiag-1));
newloc.row(0) = newloc.row(1) + curdiag;
proploc = join_horiz(proploc, newloc);
propvals = join_vert(propvals, thisdiag);
}
prop = arma::sp_mat(proploc, propvals, ylen, ylen);
// check normalization, should get all 1's
// std::cout << myk*sum(prop,0) << '\n';
haveProp = true;
return 0;
}
