void test_qtvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Vector2f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Matrix2f()));
CALL_SUBTEST(check_qtvector_matrix(Vector4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_qtvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_qtvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_qtvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_qtvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_qtvector_transform(Transform2f()));
CALL_SUBTEST(check_qtvector_transform(Transform3f()));
CALL_SUBTEST(check_qtvector_transform(Transform3d()));
//CALL_SUBTEST(check_qtvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
}
void App::control_target(Options *options)
{
if ((options->button_lerp_clicked) || (options->button_slerp_clicked))
{
if (options->button_lerp_clicked)
active_lerp = true;
if (options->button_slerp_clicked)
active_slerp = true;
options->button_lerp->set_enabled(false);
options->button_lerp_clicked = false;
options->button_slerp->set_enabled(false);
options->button_slerp_clicked = false;
time_made_active = current_time;
initial_quaternion = options->quaternion;
final_quaternion = Quaternionf(options->target_x, options->target_y, options->target_z, order_YXZ);
}
if (options->button_rotate_clicked)
{
Quaternionf quat = Quaternionf::multiply(Quaternionf(options->target_x, options->target_y, options->target_z, order_YXZ), options->quaternion);
options->set_new_quaternion(quat);
options->button_rotate_clicked = false;
}
if (active_lerp || active_slerp)
{
float time_active = ((float) (current_time - time_made_active)) / 2000.0f;
if (time_active > 1.0f)
{
time_active = 1.0f;
}
if (active_lerp)
{
Quaternionf quat = Quaternionf::lerp(initial_quaternion, final_quaternion, time_active);
options->set_new_quaternion(quat);
}
else if (active_slerp)
{
Quaternionf quat = Quaternionf::slerp(initial_quaternion, final_quaternion, time_active);
options->set_new_quaternion(quat);
}
if (time_active >= 1.0f)
{
options->button_lerp->set_enabled(true);
options->button_slerp->set_enabled(true);
active_lerp = false;
active_slerp = false;
}
}
}
void test_stdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Projective2f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3d()));
//CALL_SUBTEST(heck_stdvector_transform(Projective4d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
Rotate::Rotate(Vector3f axis, double angle){
Vector3f norm_axis = axis.normalized();
angle = angle * PI / 360;
Vector3f imag = norm_axis*sin(angle);
double real = cos(angle);
quat = Quaternionf(real, imag(0), imag(1), imag(2));
}
void test_stdvector_overload()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Affine2f())); // does not need the specialization (2+1)^2 = 9
CALL_SUBTEST_4(check_stdvector_transform(Affine3f()));
CALL_SUBTEST_4(check_stdvector_transform(Affine3d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
Quaternionfs getRotations( const GIDSet& gids ) const final
{
const float deg2rad = float( M_PI ) / 180.f;
const brion::NeuronMatrix& data =
_circuit.get( gids, brion::NEURON_ROTATION );
Quaternionfs rotations( gids.size( ));
#pragma omp parallel for
for( size_t i = 0; i < gids.size(); ++i )
{
try
{
// transform rotation Y angle in degree into rotation quaternion
const float angle = lexical_cast<float>( data[i][0] ) * deg2rad;
rotations[i] = Quaternionf( angle, Vector3f( 0, 1, 0 ));
}
catch( const boost::bad_lexical_cast& )
{
GIDSet::const_iterator gid = gids.begin();
std::advance( gid, i );
LBWARN << "Error parsing circuit orientation for gid "
<< *gid << std::endl;
}
}
return rotations;
}
void Options::update_quaternion()
{
quaternion = Quaternionf(rotation_x, rotation_y, rotation_z, order_YXZ);
write_quaternion();
update_all_slider_text();
}
void QTEntityParametersWidget::quaternionSpinBoxModified(double)
{
if(NULL == p_entity) return;
ModelTransform* transform = p_entity->getTransformation();
transform->setRotation(Quaternionf(ui.qx->value(), ui.qy->value(), ui.qz->value(), ui.qw->value()));
transform->updateAll();
updateTransformView();
}
SceneObject& SceneObject::setRotation(const Vector3f& x, const Vector3f& y, const Vector3f& z) {
Matrix3f M;
M.col(0) = x;
M.col(1) = y;
M.col(2) = z;
rotation = Quaternionf(M);
rotation.normalize();
return *this;
}
std::unique_ptr<Camera> Camera::create()
{
std::unique_ptr<Camera> p(new Camera);
p->m_center = Vector3f(0.0f, 0.0f, 0.0f);
p->m_zoom = 0.25f;
p->m_near = 0.1f;
p->m_far = 64.0f;
p->m_rotation = Quaternionf(0.0f, 0.0f, 0.0f, 1.0f);
return p;
}
void Camera::lookAt(const Vector3f ¢er, const Vector3f &eye, Vector3f &up) {
Vector3f z = eye - center;
z.normalize();
up.normalize();
Vector3f x = up.cross(z);
x.normalize();
Vector3f y = z.cross(x);
Matrix3f M;
M.col(0) = x;
M.col(1) = y;
M.col(2) = z;
std::cout << M << std::endl;
rotation = Quaternionf(M);
position = eye;
}
void test_geo_quaternion()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1(( quaternion<float,AutoAlign>() ));
CALL_SUBTEST_1( check_const_correctness(Quaternionf()) );
CALL_SUBTEST_2(( quaternion<double,AutoAlign>() ));
CALL_SUBTEST_2( check_const_correctness(Quaterniond()) );
CALL_SUBTEST_3(( quaternion<float,DontAlign>() ));
CALL_SUBTEST_4(( quaternion<double,DontAlign>() ));
CALL_SUBTEST_5(( quaternionAlignment<float>() ));
CALL_SUBTEST_6(( quaternionAlignment<double>() ));
CALL_SUBTEST_1( mapQuaternion<float>() );
CALL_SUBTEST_2( mapQuaternion<double>() );
}
}
Quaternionf Node::GetRotation(CoordSys coordSys) const
{
switch (coordSys)
{
case CoordSys_Global:
if (!m_derivedUpdated)
UpdateDerived();
return m_derivedRotation;
case CoordSys_Local:
return m_rotation;
}
NazaraError("Coordinate system out of enum (0x" + String::Number(coordSys, 16) + ')');
return Quaternionf();
}
void ModelBrowserTool::Render()
{
Renderer* renderer = GraphicSubsystem::Instance()->GetRenderer();
GD_ASSERT(renderer);
renderer->SetViewport( 0, 0, mModelBrowserWindow->mViewerFrame->width(), mModelBrowserWindow->mViewerFrame->height() );
renderer->SetClearColor( Color4f( 0.1f, 0.2f, 0.4f, 1.0f) );
renderer->SetCulling( Renderer::CullBackFace );
// Render.
renderer->Clear( Renderer::ColorBuffer | Renderer::DepthBuffer );
renderer->SetRenderState( Renderer::DepthTest, true );
// Camera.
renderer->SetMatrixMode(Renderer::ProjectionMatrix);
renderer->LoadIdentity();
renderer->Perspective(mCamera.GetFovAngle(),
(float)mModelBrowserWindow->mViewerFrame->width() / (float)mModelBrowserWindow->mViewerFrame->height(),
mCamera.GetNearView(), mCamera.GetFarView());
renderer->SetMatrixMode(Renderer::ModelViewMatrix);
renderer->LoadIdentity();
renderer->SetView( mCamera.GetPosition(), mObjectCenter - mCamera.GetPosition(), mCamera.GetUp() );
Light light;
light.mPosition = mCamera.GetPosition();
light.mAmbient = Color4f(0.1f,0.1f, 0.1f,1.0f);
light.mDiffuse = Color4f(0.9f,0.9f, 0.9f,1.0f);
light.mType = Renderer::LightPoint;
renderer->SetRenderState( Renderer::Light_i, true, 0 );
renderer->SetLight( 0, light );
renderer->SetRenderState( Renderer::Lighting, true );
renderer->SetColor(Color4f(1.0f, 1.0f, 1.0f, 1.0f));
// Change the orientation.
static Float pAngle = 0;
pAngle += 0.05f;
mModel->SetOrientation(Quaternionf( Vector3f(0,1,0), pAngle ));
renderer->Translate(mModel->GetPosition());
renderer->Rotate(mModel->GetOrientation());
mModel->Render();
}
Quaternionf ArcBall::move(Vec2f p)
{
m_EndVector = mapToSphere(p);
Vec3f perp = m_StartVector.cross(m_EndVector);
if(perp.norm() > 1.0e-5f)
{
return Quaternionf(
m_StartVector.dot(m_EndVector),
perp.x(),
perp.y(),
perp.z()
);
}
return Quaternionf::Identity();
}
Quaternionf Orientation(int i,
const std::vector<std::string>& strings,
const std::vector<float>& f)
{
Quaternionf q(1,0,0,0); // Unit quaternion
while (i<strings.size()) {
std::string c = strings[i++];
if (c == "x")
q *= angleAxis(f[i++]*Radians, Vector3f::UnitX());
else if (c == "y")
q *= angleAxis(f[i++]*Radians, Vector3f::UnitY());
else if (c == "z")
q *= angleAxis(f[i++]*Radians, Vector3f::UnitZ());
else if (c == "q") {
q *= Quaternionf(f[i+0], f[i+1], f[i+2], f[i+3]);
i+=4; }
else if (c == "a") {
q *= angleAxis(f[i+0]*Radians, Vector3f(f[i+1], f[i+2], f[i+3]).normalized());
i+=4; } }
return q;
}
//-----------------------------------------------------------------------------
// 説明: 関節回転クォータニオンの取得
// 引数:
// frm [in] フレーム番号
// jid [in] 関節インデクス
// 返り値:
// 関節回転クォータニオン
// その他:
//-----------------------------------------------------------------------------
Quaternionf CMotionData::GetRotation(size_t frm, size_t jid) const
{
if (!IsRange(frm, jid))
return Quaternionf() = Quaternionf(0.f, 0.0f, 0.f, 1.0f);
return ROT(frm, jid);
}
SceneObject &SceneObject::rotate(float dir, float up, float tilt)
{
set_orientation(get_orientation() * Quaternionf(up, dir, tilt, angle_degrees, order_YXZ));
return *this;
}
void InertialDataFilter::update(InertialData& inertialData)
{
DECLARE_PLOT("module:InertialDataFilter:expectedAccX");
DECLARE_PLOT("module:InertialDataFilter:accX");
DECLARE_PLOT("module:InertialDataFilter:expectedAccY");
DECLARE_PLOT("module:InertialDataFilter:accY");
DECLARE_PLOT("module:InertialDataFilter:expectedAccZ");
DECLARE_PLOT("module:InertialDataFilter:accZ");
// check whether the filter shall be reset
if(!lastTime || theFrameInfo.time <= lastTime)
{
if(theFrameInfo.time == lastTime)
return; // weird log file replaying?
reset();
}
if(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
{
reset();
}
// get foot positions
Pose3f leftFoot = theRobotModel.limbs[Limbs::footLeft];
Pose3f rightFoot = theRobotModel.limbs[Limbs::footRight];
leftFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
rightFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
const Pose3f leftFootInvert(leftFoot.inverse());
const Pose3f rightFootInvert(rightFoot.inverse());
// calculate rotation and position offset using the robot model (joint data)
const Pose3f leftOffset(lastLeftFoot.translation.z() != 0.f ? Pose3f(lastLeftFoot).conc(leftFootInvert) : Pose3f());
const Pose3f rightOffset(lastRightFoot.translation.z() != 0.f ? Pose3f(lastRightFoot).conc(rightFootInvert) : Pose3f());
// detect the foot that is on ground
bool useLeft = true;
if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x() != 0)
{
useLeft = theWalkingEngineOutput.speed.translation.x() > 0 ?
(leftOffset.translation.x() > rightOffset.translation.x()) :
(leftOffset.translation.x() < rightOffset.translation.x());
}
else
{
Pose3f left(mean.rotation);
Pose3f right(mean.rotation);
left.conc(leftFoot);
right.conc(rightFoot);
useLeft = left.translation.z() < right.translation.z();
}
// update the filter
const float timeScale = theFrameInfo.cycleTime;
predict(RotationMatrix::fromEulerAngles(theInertialSensorData.gyro.x() * timeScale,
theInertialSensorData.gyro.y() * timeScale, 0));
// insert calculated rotation
safeRawAngle = theInertialSensorData.angle.head<2>().cast<float>();
bool useFeet = true;
MODIFY("module:InertialDataFilter:useFeet", useFeet);
if(useFeet &&
(theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::standHigh)) &&
std::abs(safeRawAngle.x()) < calculatedAccLimit.x() && std::abs(safeRawAngle.y()) < calculatedAccLimit.y())
{
const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
Vector3f accGravOnly(usedRotation.col(0).z(), usedRotation.col(1).z(), usedRotation.col(2).z());
accGravOnly *= Constants::g_1000;
readingUpdate(accGravOnly);
}
else // insert acceleration sensor values
readingUpdate(theInertialSensorData.acc);
// fill the representation
inertialData.angle = Vector2a(std::atan2(mean.rotation.col(1).z(), mean.rotation.col(2).z()), std::atan2(-mean.rotation.col(0).z(), mean.rotation.col(2).z()));
inertialData.acc = theInertialSensorData.acc;
inertialData.gyro = theInertialSensorData.gyro;
inertialData.orientation = Rotation::removeZRotation(Quaternionf(mean.rotation));
// this keeps the rotation matrix orthogonal
mean.rotation.normalize();
// store some data for the next iteration
lastLeftFoot = leftFoot;
lastRightFoot = rightFoot;
lastTime = theFrameInfo.time;
// plots
Vector3f angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(inertialData.orientation));
PLOT("module:InertialDataFilter:angleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:angleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:angleZ", toDegrees(angleAxisVec.z()));
PLOT("module:InertialDataFilter:unfilteredAngleX", theInertialSensorData.angle.x().toDegrees());
PLOT("module:InertialDataFilter:unfilteredAngleY", theInertialSensorData.angle.y().toDegrees());
angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(mean.rotation));
PLOT("module:InertialDataFilter:interlanAngleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:interlanAngleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:interlanAngleZ", toDegrees(angleAxisVec.z()));
}
void PointcloudProc::match(const Frame& frame0, const Frame& frame1,
const Eigen::Vector3d& trans, const Eigen::Quaterniond& rot,
std::vector<cv::DMatch>& matches) const
{
PointCloud<PointXYZRGBNormal> transformed_cloud;
// First, transform the current frame. (Is this inverse?) (Or just transform the other cloud?)
//transformPointCloudWithNormals<PointXYZRGBNormal>(frame1.cloud, transformed_cloud, -trans.cast<float>(), rot.cast<float>().conjugate());
//transformPointCloudWithNormals<PointXYZRGBNormal>(frame0.pointcloud, transformed_cloud, -trans.cast<float>(), rot.cast<float>().conjugate());
transformPointCloudWithNormals<PointXYZRGBNormal>(frame0.pointcloud, transformed_cloud, Vector3f(0,0,0), rot.cast<float>().conjugate());
transformPointCloudWithNormals<PointXYZRGBNormal>(transformed_cloud, transformed_cloud, -trans.cast<float>(), Quaternionf(1, 0, 0, 0));
//pcl::io::savePCDFileASCII ("cloud0.pcd", transformed_cloud);
//pcl::io::savePCDFileASCII ("cloud1.pcd", frame1.pointcloud);
// Optional/TODO: Perform ICP to further refine estimate.
/*PointCloud<PointXYZRGBNormal> cloud_reg;
IterativeClosestPointNonLinear<PointXYZRGBNormal, PointXYZRGBNormal> reg;
reg.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGBNormal> > (transformed_cloud));
reg.setInputTarget (boost::make_shared<const PointCloud<PointXYZRGBNormal> > (frame1.pointcloud));
reg.setMaximumIterations(50);
reg.setTransformationEpsilon (1e-8);
reg.align(cloud_reg); */
// Find matches between pointclouds in frames. (TODO: also compare normals)
std::vector<int> f0_indices, f1_indices;
getMatchingIndices(transformed_cloud, frame1.pointcloud, f0_indices, f1_indices);
// Fill in keypoints and projections of relevant features.
// Currently just done when setting the pointcloud.
// Convert matches into the correct format.
matches.clear();
// Starting at i=1 as a hack to not let through (0,0,0) matches (why is this in the ptcloud?))
#pragma omp parallel for shared( transformed_cloud, f0_indices, f1_indices )
for (unsigned int i=1; i < f0_indices.size(); i++)
{
const PointXYZRGBNormal &pt0 = transformed_cloud.points[f0_indices[i]];
const PointXYZRGBNormal &pt1 = frame1.pointcloud.points[f1_indices[i]];
// Figure out distance and angle between normals
Quaterniond normalquat;
Vector3d norm0(pt0.normal[0], pt0.normal[1], pt0.normal[2]), norm1(pt1.normal[0], pt1.normal[1], pt1.normal[2]);
normalquat.setFromTwoVectors(norm0, norm1);
//double angledist = normalquat.angularDistance(normalquat.Identity());
double dist = (Vector3d(pt0.x, pt0.y, pt0.z)-Vector3d(pt1.x, pt1.y, pt1.z)).norm();
/* Vector4d p0_pt = Vector4d(pt0.x, pt0.y, pt0.z, 1.0);
Vector3d expected_proj = projectPoint(p0_pt, frame0.cam);
Vector3d diff = expected_proj - frame1.pl_kpts[f1_indices[i]].head<3>();
diff(2) = diff(2) - diff(0);
printf("[Proj difference] %f %f %f\n", diff(0), diff(1), diff(2)); */
if ((norm0 - norm1).norm() < 0.5 && dist < 0.2)
#pragma omp critical
matches.push_back(cv::DMatch(f0_indices[i], f1_indices[i], dist));
}
printf("[Frame] Found %d matches, then converted %d matches.\n", (int)f0_indices.size(), (int)matches.size());
}
system::error_code PoseEstimation::update(asio::yield_context yctx)
{
#if EKF_USE_VR
Vector3f velocityRate = Vector3f(
-m_VelocityRate.m_Value(0),
-m_VelocityRate.m_Value(1),
-m_VelocityRate.m_Value(2)
) * 9.80665f;
#else
Vector3f velocityRate = Vector3f(0.0f, 0.0f, -9.80665f);
#endif
#if EKF_USE_RR
Vector3f rotationRate = Vector3f(
m_RotationRate.m_Value(0),
m_RotationRate.m_Value(1),
m_RotationRate.m_Value(2)
);
#else
Vector3f rotationRate = Vector3f();
#endif
#if EKF_USE_MF
Vector3f magneticField = Vector3f(
-m_MagneticField.m_Value(0),
-m_MagneticField.m_Value(1),
-m_MagneticField.m_Value(2)
);
#else
Vector3f magneticField = Vector3f();//ekf22::Vector3f(0.0f, 10.0f, 0.0f);
#endif
#if EKF_USE_BA
float baroHeight = -m_RelativeAltitude.m_Value;
#else
float baroHeight = 0.0f;
#endif
m_EKFContext.time_us = std::lround(1.0e+6f*m_Time.m_Value);
uint32_t const time_ms = m_EKFContext.getMillis();
m_EKF.dtIMU = m_DT.m_Value;
m_EKF.dAngIMU = 0.5f*(m_EKF.angRate + rotationRate)*m_EKF.dtIMU;
m_EKF.angRate = rotationRate;
m_EKF.dVelIMU = 0.5f*(m_EKF.accel + velocityRate)*m_EKF.dtIMU;
m_EKF.accel = velocityRate;
m_EKF.magData = magneticField;
m_EKF.updateDtHgtFilt(m_DT.m_Value);
m_EKF.baroHgt = baroHeight;
if(!m_EKF.statesInitialised)
{
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
m_EKF.posNE[0] = 0.0f;
m_EKF.posNE[1] = 0.0f;
m_EKF.InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
}
m_EKF.setOnGround(m_bOnGround);
struct ekf_status_report ekfReport;
int ekfCheck = m_EKF.CheckAndBound(&ekfReport);
if(ekfCheck)
{
return base::makeErrorCode(base::kENoError);
}
//IMU update
m_EKF.UpdateStrapdownEquationsNED();
m_EKF.StoreStates(time_ms);
m_EKF.summedDelAng = m_EKF.summedDelAng + m_EKF.correctedDelAng;
m_EKF.summedDelVel = m_EKF.summedDelVel + m_EKF.dVelIMU;
m_EKFCovariancePredDT += m_EKF.dtIMU;
if( (m_EKFCovariancePredDT >= (m_EKF.covTimeStepMax - m_EKF.dtIMU)) ||
(m_EKF.summedDelAng.length() > m_EKF.covDelAngMax) )
{
m_EKF.CovariancePrediction(m_EKFCovariancePredDT);
m_EKF.summedDelAng.zero();
m_EKF.summedDelVel.zero();
m_EKFCovariancePredDT = 0.0f;
}
m_EKF.globalTimeStamp_ms = time_ms;
//opt flow fusion
m_EKF.fuseOptFlowData = false;
//gps fusion
m_EKF.fuseVelData = false;
m_EKF.fusePosData = false;
//barometer fusion
m_EKF.hgtMea = m_EKF.baroHgt;
m_EKF.fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
m_EKF.RecallStates(m_EKF.statesAtHgtTime, time_ms - 20);
// run the fusion step
m_EKF.FuseVelposNED();
//m_EKF.globalTimeStamp_ms = chrono::duration_cast<chrono::milliseconds>(m_IMUDuration).count();
#if 0
//mag fusion
m_EKF.fuseMagData = true;
m_EKF.RecallStates(m_EKF.statesAtMagMeasTime, time_ms - 20); // Assume 50 msec avg delay for magnetometer data
m_EKF.magstate.obsIndex = 0;
m_EKF.FuseMagnetometer();
m_EKF.FuseMagnetometer();
m_EKF.FuseMagnetometer();
#else
m_EKF.fuseMagData = false;
#endif
//airspeed fusion
m_EKF.fuseVtasData = false;
m_Attitude.m_Value = Quaternionf(m_EKF.states[0], m_EKF.states[1], m_EKF.states[2], m_EKF.states[3]);
m_Velocity.m_Value = Vec3f(m_EKF.states[4], m_EKF.states[5], m_EKF.states[6]);
m_Position.m_Value = Vec3f(m_EKF.states[7], m_EKF.states[8], m_EKF.states[9]);
//roationRateBias = Vec3f(m_EKF.states[10], m_EKF.states[11], m_EKF.states[12]);
//velocityRateZBias = m_EKF.states[13];
//wind = Vec2f(m_EKF.states[14], m_EKF.states[15]);
//earthMagneticField = Vec3f(m_EKF.states[16], m_EKF.states[17], m_EKF.states[18]);
//bodyMagneticField = Vec3f(m_EKF.states[19], m_EKF.states[20], m_EKF.states[21]);
return base::makeErrorCode(base::kENoError);
}
SceneObject& SceneObject::setRotation(const Matrix3f& M) {
rotation = Quaternionf(M);
rotation.normalize();
return *this;
}
void game::run()
{
//MissileDesc projectile(*this);
towerDesc towr(this);
std::string txt;
std::string txt2 = "Life:";
std::string txt3 = "Money:";
std::string main = "Cam Pos";
std::string txt4 = "Game Over";
std::list<SceneObject *> troll;
endgame = false;
mouseUp = false;
float dir = 0.0f;
float spot_dir = 45.0f;
float aspect_time = 0.0f;
towerCount = 0;
cost = 0;
life = 20;
money = 10;
quit = false;
cameraRestricted = false;
selected_tower = "";
std::stringstream ss2 (std::stringstream::in | std::stringstream::out);
std::stringstream ss3 (std::stringstream::in | std::stringstream::out);
/*GUI app;
int retval = app.main(args);
if (retval == 0)
{
return retval;
}*/
Quaternionf camera_orientation(55.0f, 0.0f, 0.0f, angle_degrees, order_YXZ);
clan::OpenGLWindowDescription opengl_desc;
opengl_desc.set_version(3, 2, false);
clan::OpenGLTarget::set_description(opengl_desc);
//Scene test;
//clan::SceneCamera camera(test);
//DisplayWindowDescription innerwindow("Hello World", Size(640, 480), true);
//innerwindow.set_allow_resize(true);
//innerwindow.set_fullscreen(true);
clan::DisplayWindowDescription win_desc;
win_desc.set_allow_resize(true);
win_desc.set_title("Main");
win_desc.set_size(clan::Size( 1100, 900 ), false);
DisplayWindow window(win_desc);
//DisplayWindow window("main", 1200, 900, false, true);
//window.maximize();
//DisplayWindow window(innerwindow);
Rect viewport = window.get_viewport();
GraphicContext gc = window.get_gc();
ResourceManager resources;
SceneCache::set(resources, std::shared_ptr<SceneCache>(new AppSceneCache()));
ResourceManager sound_resources = XMLResourceManager::create(XMLResourceDocument("../Resources/resources.xml"));
sound_resources_=&sound_resources;
std::string shader_path = "../Resources/Scene3D";
Scene scene(gc, resources, shader_path);
sceneHolder = &scene;
SceneCamera camera(scene);
scene.set_camera(camera);
scene.set_viewport(gc.get_size());
camera.set_orientation(camera_orientation);
camera.set_position(Vec3f(0.0f, 40.0f, down));
Canvas canvas(window);
image_hp = Image(canvas, "../Resources/Images/heart.png");
image_hp.set_alignment(origin_center);
image_hp.set_scale(0.07, 0.07);
image_coin = Image(canvas, "../Resources/Images/coin.png");
image_coin.set_alignment(origin_center);
image_coin.set_scale(0.3, 0.3);
GUIWindowManagerDirect direct(window, canvas);
GUIManager maingui(direct, "../Resources");
GUIComponent *win_comp = new GUIComponent(&maingui, win_desc, "");
radial_menu = new RadialMenu(win_comp);
radial_menu->func_selected.set(this, &game::on_radial_menu_itemselected);
//GameComponent game_component(viewport, &maingui);
/*Rect toolbar_rect = Rect((viewport.get_width() - 448) / 2, viewport.bottom - 56, (viewport.get_width() - 448) / 2 + 448, viewport.bottom);
Toolbar toolbar(toolbar_rect, &game_component); // GameComponent is the "desktop" that the toolbar sits on, as an owner
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell1.png"), Sprite(canvas, "../Resources/Images/spell1_selected.png"), Sprite(canvas, "../Resources/Images/spell1_clicked.png"));
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell2.png"), Sprite(canvas, "../Resources/Images/spell2_selected.png"), Sprite(canvas, "../Resources/Images/spell2_clicked.png"));
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell3.png"), Sprite(canvas, "../Resources/Images/spell3_selected.png"), Sprite(canvas, "../Resources/Images/spell3_clicked.png"));
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell4.png"), Sprite(canvas, "../Resources/Images/spell4_selected.png"), Sprite(canvas, "../Resources/Images/spell4_clicked.png"));
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell5.png"), Sprite(canvas, "../Resources/Images/spell5_selected.png"), Sprite(canvas, "../Resources/Images/spell5_clicked.png"));
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell6.png"), Sprite(canvas, "../Resources/Images/spell6_selected.png"), Sprite(canvas, "../Resources/Images/spell6_clicked.png"));
toolbar.add_item(Sprite(canvas, "../Resources/Images/spell7.png"), Sprite(canvas, "../Resources/Images/spell7_selected.png"), Sprite(canvas, "../Resources/Images/spell7_clicked.png"));
*/
InputDevice keyboard = window.get_ic().get_keyboard();
InputDevice mouse = window.get_ic().get_mouse();
clan::Font font(canvas, "Tahoma", 30); // The clan prefix is required on linux due to a namespace conflict
SceneModel plane(gc, scene, "plane");
SceneModel box(gc, scene, "box");
SceneModel tower(gc, scene, "tower");
SceneModel gate(gc,scene,"gate");
SceneObject object(scene, plane, Vec3f(0.0f, 0.0f, 0.0f));
object.rotate(180.0f, 0.0f, 0.0f);
scene.show_skybox_stars(false);
/*SceneObject box0(scene, tower, Vec3f(20.0f, 5.0f, 0.0f), Quaternionf(0.0f, 0.0f, 0.0f, angle_degrees, order_YXZ));
SceneObject box1(scene, tower, Vec3f(-20.0f, 5.0f, 0.0f), Quaternionf(0.0f, 0.0f, 0.0f, angle_degrees, order_YXZ));
SceneObject box2(scene, tower, Vec3f(0.0f, 5.0f, 20.0f), Quaternionf(0.0f, 0.0f, 0.0f, angle_degrees, order_YXZ));
SceneObject box3(scene, tower, Vec3f(0.0f, 5.0f, -20.0f), Quaternionf(0.0f, 0.0f, 0.0f, angle_degrees, order_YXZ));
SceneObject tower[10];*/
SceneObject theGate(scene, gate, Vec3f(-75.0f,5.0f,60.0f), Quaternionf(0.0f,0.0f,0.0f,angle_degrees,order_YXZ));
theGate.set_scale(Vec3f(1.0f,2.0f,2.0f));
std::vector<SceneLight> omni_lights;
for (int i = 0; i < 4; i++)
{
SceneLight omni(scene);
omni.set_type(SceneLight::type_omni);
omni.set_color(Vec3f(0.05f));
omni.set_position(Quaternionf(45.0f, 45.0f + i * 90.0f, 0.0f, angle_degrees, order_YXZ).rotate_vector(Vec3f(0.0f, 0.0f, -100.0f)));
omni.set_attenuation_end(200.0f);
omni.set_ambient_illumination(0.025f);
omni_lights.push_back(omni);
}
SceneLight spot(scene);
spot.set_type(SceneLight::type_spot);
spot.set_orientation(Quaternionf(45.0f, 45.0f, 0.0f, angle_degrees, order_YXZ));
spot.set_position(spot.get_orientation().rotate_vector(Vec3f(-55.0f, 90.0f, -550.0f)));
//spot.set_position(Vec3f(-250, 100, -430));
spot.set_color(Vec3f(1.0f, 1.0f, 0.8f));
spot.set_falloff(45.0f);
spot.set_hotspot(15.0f);
spot.set_attenuation_start(150.0f);
spot.set_attenuation_end(1500.0f);
spot.set_shadow_caster(true);
spot.set_rectangle_shape(false);
//spot.set_ambient_illumination(true);
spot.set_aspect_ratio(1.0f);
/*SceneLight spot(scene);
spot.set_type(SceneLight::type_spot);
spot.set_orientation(Quaternionf(30.0f, 30.0f, 0.0f, angle_degrees, order_YXZ));
spot.set_position(spot.get_orientation().rotate_vector(Vec3f(0.0f, 0.0f, -100.0f)));
spot.set_color(Vec3f(1.0f, 1.0f, 0.8f));
spot.set_falloff(45.0f);
spot.set_hotspot(15.0f);
spot.set_attenuation_start(20.0f);
spot.set_attenuation_end(200.0f);
spot.set_shadow_caster(true);
spot.set_rectangle_shape(false);
spot.set_aspect_ratio(1.0f);*/
/*SceneLight spot2(scene);
spot2.set_type(SceneLight::type_spot);
spot2.set_position(Vec3f(0.0f, 100.0f, 0.0f));
spot2.set_color(Vec3f(1.0f, 1.0f, 1.0f) * 3.0f);
spot2.set_falloff(35.0f);
spot2.set_hotspot(30.0f);
spot2.set_attenuation_start(20.0f);
spot2.set_attenuation_end(130.0f);
spot2.set_shadow_caster(true);
spot2.set_rectangle_shape(false);
spot2.set_ambient_illumination(0.025f);*/
wm.NodeArray[0].setPos(75.0f,1.25f,0.0f);
wm.NodeArray[1].setPos(-60.0f,1.25f,0.0f);
wm.NodeArray[2].setPos(-55.0f,1.25f,-65.0f);
wm.NodeArray[3].setPos(60.0f,1.25f,-60.0f);
wm.NodeArray[4].setPos(55.0f,1.25f,65.0f);
wm.NodeArray[5].setPos(-80.0f,1.25f,60.0f);
Physics3DShape box_shape = Physics3DShape::box(Vec3f(5.0f));
Physics3DShape plane_shape = Physics3DShape::box(Vec3f(75.0f, 1.0f, 75.0f));
Physics3DShape sphere_shape = Physics3DShape::sphere(2.0f);
Physics3DObject phys_plane(physics_world, plane_shape, Vec3f(0.0f, -0.5f, 0.0f));
/*Physics3DObject phys_box0(physics_world, box_shape, Vec3f(20.0f, 5.0f, 0.0f), box0.get_orientation());
Physics3DObject phys_box1(physics_world, box_shape, Vec3f(-20.0f, 5.0f, 0.0f), box1.get_orientation());
Physics3DObject phys_box2(physics_world, box_shape, Vec3f(0.0f, 5.0f, 20.0f), box2.get_orientation());
Physics3DObject phys_box3(physics_world, box_shape, Vec3f(0.0f, 5.0f, -20.0f), box3.get_orientation());
Physics3DObject phys_box[10];*/
Physics3DSweepTest sweep_test(physics_world);
Physics3DRayTest raycast(physics_world);
Slot slot_keyboard_key_down = (window.get_ic().get_keyboard()).sig_key_down() .connect(this,&game::on_key_down);
Slot slot_keyboard_key_up = (window.get_ic().get_keyboard()).sig_key_up() .connect(this,&game::on_key_up);
Slot slot_mouse_moved = (window.get_ic().get_mouse()).sig_pointer_move() .connect(this,&game::on_pointer_move);
Slot slot_mouse_down = (window.get_ic().get_mouse()).sig_key_down() .connect(this,&game::on_pointer_down);
Slot slot_mouse_up = (window.get_ic().get_mouse()).sig_key_up() .connect(this,&game::on_pointer_up);
//________________________________________________________________
// S O U N D S
total_channels=3;
current_channel=1;
SoundBuffer music = SoundBuffer::resource("Music1",sound_resources);
music.set_volume(0.3f);
sound_session1.play();
sound_session2.play();
sound_session3.play();
total_samples = 6;
samples.resize(total_samples);
samples[0] = SoundBuffer::resource("Explosion1",sound_resources);
samples[1] = SoundBuffer::resource("Explosion2",sound_resources);
samples[2] = SoundBuffer::resource("Hurt1",sound_resources);
samples[3] = SoundBuffer::resource("Hurt2",sound_resources);
samples[4] = SoundBuffer::resource("Powerup1",sound_resources);
samples[5] = SoundBuffer::resource("Shoot1",sound_resources);
for(int i = 0; i<total_samples; i++)
{
samples[i].set_volume(0.3f);
}
SoundBuffer_Session music_session = music.prepare();
music_session_ = &music_session;
music_session.set_looping(true);
music_session.play();
is_music_muted = false;
/********Enemies(Jasper)************************/
SceneObject enemyobj[30];
wm.spawnCreeps();
for(int i = 0; i < 30; i ++)
{
enemyobj[i] = SceneObject(scene, box);
enemyobj[i].set_scale(Vec3f(0.25f));
wm.enemyArray[i].setEnemyObject(&enemyobj[i]);
}
/***************************************/
ElapsedTimer elapsed_timer;
while (!quit)
{
float time_elapsed = elapsed_timer.seconds_elapsed();
/*spot_dir = std::fmod(spot_dir + time_elapsed * 30.0f, 90.0f);
aspect_time = std::fmod(aspect_time + time_elapsed * 0.2f, 2.0f);
spot2.set_aspect_ratio(clamp(aspect_time >= 1.0f ? 2.0f - aspect_time : aspect_time, 0.1f, 1.0f));
spot2.set_orientation(Quaternionf(65.0f + (spot_dir >= 45.0f ? 90.0f - spot_dir : spot_dir), 60.0f, dir * 4.0f, angle_degrees, order_YXZ));
*/
// Draw with the canvas:
/*canvas.clear(Colorf::cadetblue);
canvas.draw_line(0, 110, 640, 110, Colorf::yellow);
font.draw_text(canvas, 100, 100, "Hello World!", Colorf::lightseagreen);
// Draw any remaining queued-up drawing commands oQAn canvas:
canvas.flush();*/
if (!endgame)
{
if (mouse.get_x() <= 0 && !cameraRestricted)
{
mouse.set_position(0, mouse.get_y());
camera.set_position(camera.get_position()+Vec3f(-mouse_move_speed, 0.0f, 0.0f));
/*canvas.clear();
font.draw_text(canvas, 100, 100, "x=0", Colorf::lightseagreen);
canvas.flush();*/
//window.flip();
}
if (mouse.get_y() <= 0 && !cameraRestricted)
{
mouse.set_position(mouse.get_x(), 0);
camera.set_position(camera.get_position()+Vec3f(0.0f, 0.0f, mouse_move_speed));
/*canvas.clear();
font.draw_text(canvas, 100, 100, "y=0", Colorf::lightseagreen);
canvas.flush();*/
//window.flip();
}
if (mouse.get_x() >= window.get_gc().get_width()-1 && !cameraRestricted)
{
mouse.set_position(window.get_gc().get_width()-1, mouse.get_y());
camera.set_position(camera.get_position()+Vec3f(mouse_move_speed, 0.0f, 0.0f));
/*canvas.clear();
font.draw_text(canvas, 100, 100, "x=windowRight", Colorf::lightseagreen);
canvas.flush();*/
//window.flip();
}
if (mouse.get_y() >= window.get_gc().get_height()-1 && !cameraRestricted)
{
mouse.set_position(mouse.get_x(), window.get_gc().get_height()-1);
camera.set_position(camera.get_position()+Vec3f(0.0f, 0.0f, -mouse_move_speed));
/*canvas.clear();
font.draw_text(canvas, 100, 100, "y=windowBottom", Colorf::lightseagreen);
canvas.flush();*/
//window.flip();
}
if (mouseUp)
{
if (selected_tower == "Tower 1")
{
towr.set_type(towr.t_bullet);
cost = 10;
}
else if (selected_tower == "Tower 2")
{
towr.set_type(towr.t_energy);
cost = 15;
}
else if (selected_tower == "Tower 3")
{
towr.set_type(towr.t_rocket);
cost = 20;
}
scene.unproject(mouse_pos, start, end);
end *= 151;
test = start + end;
if (raycast.test(start, test))
{
if (towerCount < 10 && money >= cost)
{
towr.set_pos(Vec3f(raycast.get_hit_position().x, 5.0f, raycast.get_hit_position().z));
towr.create(scene, tower, physics_world);
money -= cost;
towerCount++;
cost = 0;
}
}
mouseUp = false;
//float x = mouse.get_x() - scene.world_to_projection().get_origin_x();
//tower[0] = SceneObject(scene, box, Vec3f(camera.get_position().x, 5.0f, camera.get_position().z));
//tower[0].set_position(Vec3f(0.0f, 5.0f, 0.0f));
//window.flip();
//canvas.clear(Colorf::cadetblue);
/*canvas.draw_line(0, 110, 640, 110, Colorf::yellow);
font.draw_text(canvas, 100, 100, "Hello World!", Colorf::lightseagreen);
// Draw any remaining queued-up drawing commands oQAn canvas:
canvas.flush();
// Present the frame buffer content to the user:
window.flip();*/
}
if (mouse.get_keycode(0))
{
//maingui.process_messages(0);
//test.update();
//show_radial_menu(mouse.get_position());
}
/*if (mouse.get_keycode(1))
{
scene.unproject(mouse.get_position(), start, end);
end *= 151;
Vec3f test = start + end;
if (raycast.test(start, test))
{
Physics3DObject hit = raycast.get_hit_object();
font.draw_text(canvas, 400, 100, "hit", Colorf::lightseagreen);
//hit.set_position(Vec3f(0.0f, -10.0f, 0.0f));
std::stringstream ss3 (std::stringstream::in | std::stringstream::out);
ss3 << hit.get_position().y;
txt3 = ss3.str();
font.draw_text(canvas, 550, 100, txt3, Colorf::lightseagreen);
}
//projectile.set_pos(Vec3f(raycast.get_hit_position().x, 5.0f, raycast.get_hit_position().z));
//projectile.fire(scene, box, physics_world);
//canvas.clear(Colorf::white);
//window.flip();
}*/
if (keyboard.get_keycode(keycode_control))
{
if (window.is_fullscreen())
{
//innerwindow.set_fullscreen(false);
window.set_size(640, 480, true);
window.restore();
}
else
window.set_size(640, 480, false);
}
if (camera.get_position().x <= left)
{
camera.set_position(Vec3f(left, camera.get_position().y, camera.get_position().z));
}
if (camera.get_position().x >= right)
{
camera.set_position(Vec3f(right, camera.get_position().y, camera.get_position().z));
}
if (camera.get_position().z >= top)
{
camera.set_position(Vec3f(camera.get_position().x, camera.get_position().y, top));
}
if (camera.get_position().z <= down)
{
camera.set_position(Vec3f(camera.get_position().x, camera.get_position().y, down));
}
/*if(objects_for_deletion.size()>0)
{
std::list<Gameobject *>::iterator it;
for(it=objects_for_deletion.begin(); it!= objects_for_deletion.end(); ++it)
{
delete (*it);
//towerCount--;
}
objects_for_deletion.clear();
}*/
if(towerObjects.size()>0)
{
std::vector<Gameobject *>::iterator it;
int counter = 0;
for(it=towerObjects.begin(); it!= towerObjects.end(); ++it)
{
towerObjects.at(counter)->update(time_elapsed, wm.enemyArray, 30);
counter++;
}
}
if(missileObjects.size()>0)
{
std::list<Gameobject *>::iterator it;
for(it=missileObjects.begin(); it!= missileObjects.end();)
{
//if (!(*it)->checkActive())
(*it)->update(time_elapsed, wm.enemyArray);
if ((*it)->checkActive())
{
delete(*it);
it = missileObjects.erase(it);
}
else
it++;
}
}
/*******Enemies(Updates)************************/
wm.Update(1);
//time_lasttime = time_elapsed;
if(wm.life > 0)
{
// Put a screen here
life -= wm.life;
wm.life = 0;
}
if (wm.currLevel == 3)
{
endgame = true;
}
/***********************************************/
//scene.update(gc, time_elapsed);
txt3 = ":";
ss3.str("");
ss3.clear();
ss3 << money;
txt3 += ss3.str();
font.draw_text(canvas, 550, 50, txt3, Colorf::lightseagreen);
if (life < 0)
{
endgame = true;
}
scene.render(gc);
/*canvas.clear(clan::Colorf(0.0f,0.0f,0.0f));
std::stringstream ss (std::stringstream::in | std::stringstream::out);
ss << game_time.get_time_elapsed();
std::string troll = ss.str();
font.draw_text(canvas, 100, 100, troll, Colorf::lightseagreen);*/
//canvas.clear(clan::Colorf(0.0f,0.0f,0.0f));
std::stringstream ss (std::stringstream::in | std::stringstream::out);
ss << scene.world_to_projection().get_origin_x();
txt = ss.str();
txt2 = ":";
ss2.str("");
ss2.clear();
ss2 << life;
txt2 += ss2.str();
font.draw_text(canvas, 150, 50, txt2, Colorf::lightseagreen);
//maingui.process_messages(0);
/*font.draw_text(canvas, 0, 100, main, Colorf::lightseagreen);
font.draw_text(canvas, 150, 100, txt, Colorf::lightseagreen);
font.draw_text(canvas, 250, 100, txt2, Colorf::lightseagreen);
canvas.draw_line(0, 110, 640, 110, Colorf::yellow);*/
maingui.render_windows();
image_hp.draw(canvas, 100, 45);
image_coin.draw(canvas, 500, 45);
window.flip();
// Read messages from the windowing system message queue, if any are available:
KeepAlive::process();
}
else
{
//maingui.render_windows();
canvas.clear(Colorf::black);
font.draw_text(canvas, 450, 400, txt4, Colorf::lightseagreen);
window.flip();
// Read messages from the windowing system message queue, if any are available:
KeepAlive::process();
}
}
}
SceneObject& SceneObject::setRotation(const AngleAxisf &angleAxis) {
rotation = Quaternionf(angleAxis);
rotation.normalize();
return *this;
}
system::error_code AttitudeControlSimple::update(asio::yield_context yctx)
{
Mat3x3f R = m_ISAttitudeEstimated.m_Value.toRotationMatrix();
#if 1
Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
#else
#if 0
Mat3x3f R_sp = Quaternionf(AngleAxisf(M_PI_4, Vec3f::UnitX())).toRotationMatrix();
#else
static once_flag initSetpoint;
call_once(initSetpoint, [this]() { m_ISAttitudeSetpoint.m_Value = m_ISAttitudeEstimated.m_Value; } );
Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
#endif
#endif
float dT = std::min(std::max(m_ISDT.m_Value, 0.002f), 0.02f);
Vec3f R_z(R(0, 2), R(1, 2), R(2, 2));
Vec3f R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
Vec3f e_R = R.transpose() * (R_z.cross(R_sp_z));
float e_R_z_sin = e_R.norm();
float e_R_z_cos = R_z.dot(R_sp_z);
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
Mat3x3f R_rp;
if(e_R_z_sin > 0.0f)
{
float e_R_z_angle = std::atan2(e_R_z_sin, e_R_z_cos);
Vec3f e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_angle * e_R_z_axis;
Mat3x3f e_R_cp = Mat3x3f::Zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
R_rp = R * (Mat3x3f::Identity() + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
}
else
{
R_rp = R;
}
Vec3f R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
Vec3f R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = std::atan2(R_rp_x.cross(R_sp_x).dot(R_sp_z), R_rp_x.dot(R_sp_x)) * yaw_w;
if(e_R_z_cos < 0.0f)
{
Quaternionf q(R.transpose() * R_sp);
Vec3f e_R_d = q.vec();
e_R_d.normalize();
e_R_d *= 2.0f * std::atan2(e_R_d.norm(), q.w());
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
Vec3f const rotationRateSetpoint = m_AttitudeP.cwiseProduct(e_R);
Vec3f e_RR = rotationRateSetpoint - m_ISRotationRateMeasured.m_Value;
Vec3f rotationRateControl =
m_RotationRateP.cwiseProduct(e_RR) +
m_RotationRateD.cwiseProduct(m_RotationRateMeasuredPrev - m_ISRotationRateMeasured.m_Value) / dT +
m_RotationRateIError;
m_RotationRateMeasuredPrev = m_ISRotationRateMeasured.m_Value;
m_RotationRateSetpointPrev = rotationRateSetpoint;
m_OSRotationRateSetpoint.m_Value = rotationRateControl;
return base::makeErrorCode(base::kENoError);
}
