MlcpGaussSeidelSolver.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_MLCPGAUSSSEIDELSOLVER_H
#define SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H

#include <memory.h>

#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/MlcpProblem.h"
#include "SurgSim/Math/MlcpSolver.h"
#include "SurgSim/Math/MlcpSolution.h"
#include "SurgSim/Math/Vector.h"

namespace SurgSim
{
namespace Framework
{
class Logger;
}

namespace Math
{

class MlcpGaussSeidelSolver : public MlcpSolver
{
public:
    MlcpGaussSeidelSolver();

    MlcpGaussSeidelSolver(double epsilonConvergence, double contactTolerance, size_t maxIterations);

    virtual ~MlcpGaussSeidelSolver();

    bool solve(const MlcpProblem& problem, MlcpSolution* solution);

    double getEpsilonConvergence() const;

    void setEpsilonConvergence(double precision);

    double getContactTolerance() const;

    void setContactTolerance(double tolerance);

    size_t getMaxIterations() const;

    void setMaxIterations(size_t maxIterations);

private:
    void computeEnforcementSystem(size_t problemSize, const MlcpProblem::Matrix& A,
                                  const MlcpProblem::Vector& b,
                                  const MlcpSolution::Vector& initialGuessAndSolution,
                                  const std::vector<MlcpConstraintType>& constraintsType,
                                  size_t constraintID, size_t matrixEntryForConstraintID);

    void calculateConvergenceCriteria(size_t problemSize, const MlcpProblem::Matrix& A,
                                      const MlcpProblem::Vector& b,
                                      const MlcpSolution::Vector& initialGuessAndSolution,
                                      const std::vector<MlcpConstraintType>& constraintsType,
                                      double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES],
                                      double* convergenceCriteria,
                                      bool* validSignorini);

    void doOneIteration(size_t problemSize, const MlcpProblem::Matrix& A,
                        const MlcpProblem::Vector& b,
                        MlcpSolution::Vector* initialGuessAndSolution,
                        const MlcpProblem::Vector& frictionCoefs,
                        const std::vector<MlcpConstraintType>& constraintsType,
                        double constraintConvergenceCriteria[MLCP_NUM_CONSTRAINT_TYPES], double* convergenceCriteria,
                        bool* validSignorini);

    void printViolationsAndConvergence(size_t problemSize, const MlcpProblem::Matrix& A,
                                       const MlcpProblem::Vector& b,
                                       const MlcpSolution::Vector& initialGuessAndSolution,
                                       const std::vector<MlcpConstraintType>& constraintsType,
                                       double convergenceCriteria,
                                       bool validSignorini, size_t iterations);

    double m_epsilonConvergence;

    double m_contactTolerance;

    size_t m_maxIterations;

    size_t m_numEnforcedAtomicConstraints;

    Matrix m_lhsEnforcedLocalSystem;

    Vector m_rhsEnforcedLocalSystem;

    std::shared_ptr<SurgSim::Framework::Logger> m_logger;
};

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

#endif // SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H