// update the camera state given a new camera pose
void MotionModel::UpdateCameraPose(const cv::Point3d& newPosition, const cv::Vec4d& newOrientation)
{
// Compute linear velocity
cv::Vec3d newLinearVelocity( newPosition - position_ );
// In order to be robust against fast camera movements linear velocity is smoothed over time
newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5;
// compute rotation between q1 and q2: q2 * qInverse(q1);
Eigen::Quaterniond newAngularVelocity = cv2eigen( newOrientation ) * orientation_.inverse();
// In order to be robust against fast camera movements angular velocity is smoothed over time
newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_);
newAngularVelocity.normalize();
// Update the current state variables
position_ = newPosition;
orientation_ = cv2eigen( newOrientation );
linearVelocity_ = newLinearVelocity;
angularVelocity_ = newAngularVelocity;
}
// client / slave code
void DynamicActorReplicaComponent::deserialize(RakNet::DeserializeParameters *deserializeParameters)
{
// a little base class magic
SLBaseClass::deserialize(deserializeParameters);
RakNet::BitStream& bitStream(deserializeParameters->serializationBitstream[0]);
if (bitStream.GetNumberOfBitsUsed()==0) {
return;
}
//Old Way:
//bitStream.ReadAlignedBytes( (unsigned char *)&_actorDatagram, sizeof(Dynamic2DActorDatagram) );
Compressed_Dynamic2DActorDatagram comValues;
bitStream.ReadAlignedBytes( (unsigned char *)&comValues, sizeof(Compressed_Dynamic2DActorDatagram) );
_actorDatagram._x = comValues._x;
_actorDatagram._y = comValues._y;
_actorDatagram._fx = TCompressedFixpoint<float,char,8>::readInflate(comValues._fx, -1.0f, 1.0f );
_actorDatagram._fy = TCompressedFixpoint<float,char,8>::readInflate(comValues._fy, -1.0f, 1.0f );
_killCount = _actorDatagram._killCount = comValues._killCount;
_actorDatagram._lvx = comValues._lvx;
_actorDatagram._lvy = comValues._lvy;
_actorDatagram._avz = comValues._avz;
_actorDatagram._updateTick = comValues._updateTick;
_killCount = _actorDatagram._killCount;
if(getActorSprite() == nullptr) {
return;
}
AbstractVehicle* vehicle(getActorSprite()->getVehicle());
if(vehicle != nullptr) {
Vec3 serverPosition(
_actorDatagram._x,
_actorDatagram._y,
0.0f
);
Vec3 serverForward(
_actorDatagram._fx,
_actorDatagram._fy,
0.0f
);
Vec3 serverLinearVelocity(
_actorDatagram._lvx,
_actorDatagram._lvy,
0.0f
);
Vec3 serverAngularVelocity(
0.0f,
0.0f,
_actorDatagram._avz
);
float linearInterpolationFactor(0.95f);
Vec3 newPosition(ProtocolUtilities::interpolateNetVector(vehicle->position(),
serverPosition, linearInterpolationFactor));
Vec3 newForward(ProtocolUtilities::interpolateNetVector(vehicle->forward(),
serverForward, linearInterpolationFactor));
Vec3 newLinearVelocity(ProtocolUtilities::interpolateNetVector(vehicle->linearVelocity(),
serverLinearVelocity, linearInterpolationFactor));
Vec3 newAngularVelocity(ProtocolUtilities::interpolateNetVector(vehicle->angularVelocity(),
serverAngularVelocity, linearInterpolationFactor));
vehicle->setPosition(newPosition);
vehicle->regenerateLocalSpace(newForward, 0.0f);
vehicle->setLinearVelocity(newLinearVelocity);
vehicle->setAngularVelocity(newAngularVelocity);
// very important to notify the vehicle that
// it's internal state has been changed from the outside
// see PhysicsVehicle::update
vehicle->setDirty();
_localSpaceData = vehicle->getLocalSpaceData();
_motionState.readLocalSpaceData(_localSpaceData);
}
}
