void assert_near(const Location &a,const Location &b) {
bool fail=false;
std::ostringstream str;
if (!check_near(a.getPosition(), b.getPosition())) {
str << "Positions "<<a.getPosition()<<" and "<<
b.getPosition()<<" differ."<<std::endl;
fail=true;
}
if (!check_near(a.getOrientation(), b.getOrientation())) {
str << "Orientations "<<a.getOrientation()<<" and "<<
b.getOrientation()<<" differ."<<std::endl;
fail=true;
}
if (!check_near(a.getVelocity(), b.getVelocity())) {
str << "Velocities "<<a.getVelocity()<<" and "<<
b.getVelocity()<<" differ."<<std::endl;
fail=true;
}
if (!check_near(a.getAxisOfRotation(), b.getAxisOfRotation())) {
str << "Axes "<<a.getAxisOfRotation()<<" and "<<
b.getAxisOfRotation()<<" differ."<<std::endl;
fail=true;
}
if (!check_near(a.getAngularSpeed(), b.getAngularSpeed())) {
str << "Rotation speeds "<<a.getAngularSpeed()<<" and "<<
b.getAngularSpeed()<<" differ."<<std::endl;
fail=true;
}
if (fail) {
TS_FAIL(str.str());
}
}
void changeToWorld(const Location &reference) {
setVelocity(reference.getVelocity() + reference.getOrientation() * getVelocity());
addAngularRotation(reference.getAxisOfRotation(), reference.getAngularSpeed());
setVelocity(getVelocity() +
reference.getAngularSpeed() * (
reference.getAxisOfRotation().cross(Vector3<float32>(getPosition()))));
Transform temp = Transform::toWorld(reference);
setPosition(temp.getPosition());
setOrientation(temp.getOrientation());
}
void changeToLocal(const Location &reference) {
Transform temp = Transform::toLocal(reference);
setPosition(temp.getPosition());
setOrientation(temp.getOrientation());
setVelocity(getVelocity() -
reference.getAngularSpeed() * (
reference.getAxisOfRotation().cross(Vector3<float32>(getPosition()))));
addAngularRotation(reference.getAxisOfRotation(), -reference.getAngularSpeed());
Quaternion inverseOtherOrientation (reference.getOrientation().inverse());
setVelocity(inverseOtherOrientation * (getVelocity() - reference.getVelocity()));
}
Location blend(const Location&newLocation,float32 percentNew) const{
float32 percentOld=(1.0f-percentNew);
Vector3<float32> angAxis=mAxisOfRotation*mAngularSpeed*percentOld;
angAxis+=newLocation.getAxisOfRotation()*newLocation.getAngularSpeed()*percentNew;
float angSpeed=angAxis.length();
if (angSpeed) angAxis/=angSpeed;
return Location (newLocation.getPosition()*percentNew+getPosition()*percentOld,
(newLocation.getOrientation()*percentNew+getOrientation()*percentOld).normal(),
newLocation.getVelocity()*percentNew+getVelocity()*percentOld,
angAxis,
angSpeed);
}
bool HostedObject::objectHostConnect(const SpaceID spaceID,
const Location startingLocation,
const BoundingSphere3f meshBounds,
const String mesh,
const String physics,
const String query,
const String zernike,
const ObjectReference orefID,
PresenceToken token)
{
ObjectReference oref = (orefID == ObjectReference::null()) ? ObjectReference(UUID::random()) : orefID;
SpaceObjectReference connectingSporef (spaceID,oref);
// Note: we always use Time::null() here. The server will fill in the
// appropriate value. When we get the callback, we can fix this up.
Time approx_server_time = Time::null();
if (mObjectHost->connect(
getSharedPtr(),
connectingSporef, spaceID,
TimedMotionVector3f(approx_server_time, MotionVector3f( Vector3f(startingLocation.getPosition()), startingLocation.getVelocity()) ),
TimedMotionQuaternion(approx_server_time,MotionQuaternion(startingLocation.getOrientation().normal(),Quaternion(startingLocation.getAxisOfRotation(),startingLocation.getAngularSpeed()))), //normalize orientations
meshBounds,
mesh,
physics,
query,
zernike,
std::tr1::bind(&HostedObject::handleConnected, getWeakPtr(), mObjectHost, _1, _2, _3),
std::tr1::bind(&HostedObject::handleMigrated, getWeakPtr(), _1, _2, _3),
std::tr1::bind(&HostedObject::handleStreamCreated, getWeakPtr(), _1, _2, token),
std::tr1::bind(&HostedObject::handleDisconnected, getWeakPtr(), _1, _2)
)) {
mObjectHost->registerHostedObject(connectingSporef,getSharedPtr());
return true;
}else {
return false;
}
}
bool operator() (const Location&l) const {
return l.getVelocity()==Vector3f(0.0,0.0,0.0)
&&(l.getAxisOfRotation()==Vector3f(0.0,0.0,0.0)
|| l.getAngularSpeed()==0.0);
}