MlcpPhysicsProblem.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_PHYSICS_MLCPPHYSICSPROBLEM_H
#define SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H

#include "SurgSim/Math/MlcpProblem.h"
#include "SurgSim/Math/SparseMatrix.h"

namespace SurgSim
{
namespace Physics
{


struct MlcpPhysicsProblem : public SurgSim::Math::MlcpProblem
{
    ~MlcpPhysicsProblem() override;

    Eigen::SparseMatrix<double, Eigen::RowMajor, ptrdiff_t> H;

    Matrix CHt;

    void updateConstraint(
        const Eigen::SparseVector<double, Eigen::RowMajor, ptrdiff_t>& newSubH,
        const Vector& newCHt,
        size_t indexSubC,
        size_t indexNewSubH);

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

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

};  // namespace Physics
};  // namespace SurgSim

#endif  // SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_H