Plane.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

namespace ct {
namespace core {

class Plane
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Plane() { coefficients_.setZero(); }

    Plane(const Eigen::Matrix<double, 4, 1>& coefficients) : coefficients_(coefficients) {}

    Plane(double a, double b, double c, double d) { coefficients_ << a, b, c, d; }

    Eigen::Matrix<double, 4, 1>& getCoefficients() { return coefficients_; }

    double& getCoefficient(size_t i)
    {
        if (i >= 4)
            throw std::runtime_error("Index out of range, should be max 3.");
        return coefficients_(i);
    }

    double a() { return coefficients_[AIdx]; }
    double b() { return coefficients_[BIdx]; }
    double c() { return coefficients_[CIdx]; }
    double d() { return coefficients_[DIdx]; }
    void set(double a, double b, double c, double d) { coefficients_ << a, b, c, d; };

    double solveX(double y, double z) { return (d() - b() * y - c() * z) / a(); }

    double solveY(double x, double z) { return (d() - a() * x - c() * z) / b(); }

    double solveZ(double x, double y) { return (d() - a() * x - b() * y) / c(); }
private:
    enum
    {
        AIdx = 0,  
        BIdx = 1,  
        CIdx = 2,  
        DIdx = 3   
    };

    Eigen::Matrix<double, 4, 1> coefficients_;  
};

}  // namespace core
}  // namespace ct


#pragma once