MlcpProblem.h

// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SURGSIM_MATH_MLCPPROBLEM_H
#define SURGSIM_MATH_MLCPPROBLEM_H

#include <vector>
#include <Eigen/Core>
#include "SurgSim/Math/MlcpConstraintType.h"


namespace SurgSim
{
namespace Math
{

//
// TODO(advornik): Describe the approach to friction in more detail.
// TODO(advornik): Get rid of the constraint types and encode necessary info in other ways.
struct MlcpProblem
{
    virtual ~MlcpProblem();

    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;

    Matrix A;
    Vector b;
    Vector mu;
    std::vector<MlcpConstraintType> constraintTypes;

    // NB: We let the compiler generate the default code for the constructor, copy constructor and copy assignment,
    // because we currently sometimes need to copy the problem (although we ought to minimize this).
    // The C++11-ish way to indicate that explicitly would be to write code like this:
    //     MlcpProblem() = default;
    //     MlcpProblem(const MlcpProblem& other) = default;
    //     MlcpProblem& operator= (const MlcpProblem& other) = default;
    // but I haven't yet tested that this works correctly on VS 2010, so I'm just putting in the comment.
    // We may also want to add move construction and move assignment.  --advornik 2013-06-24

    size_t getSize() const
    {
        return (b.rows() >= 0) ? static_cast<size_t>(b.rows()) : 0;
    }

    bool isConsistent() const
    {
        size_t numConstraintTypes = constraintTypes.size();
        return ((b.rows() >= 0) && (b.cols() == 1) && (A.rows() == b.rows()) && (A.cols() == A.rows())
                && (numConstraintTypes <= static_cast<size_t>(b.rows())) && (mu.size() >= 0));
    }

    virtual void setZero(size_t numDof, size_t numConstraintDof, size_t numConstraints);

    static MlcpProblem Zero(size_t numDof, size_t numConstraintDof, size_t numConstraints);
};

};  // namespace Math
};  // namespace SurgSim

#endif // SURGSIM_MATH_MLCPPROBLEM_H