1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106
|
// 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.
#include "SurgSim/Physics/SolveMlcp.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ContactConstraintData.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
namespace SurgSim
{
namespace Physics
{
SolveMlcp::SolveMlcp(bool doCopyState) : Computation(doCopyState)
{
}
SolveMlcp::~SolveMlcp()
{}
std::shared_ptr<PhysicsManagerState> SolveMlcp::doUpdate(const double& dt,
const std::shared_ptr<PhysicsManagerState>& state)
{
std::shared_ptr<PhysicsManagerState> result = state;
// Solve the Mlcp using a Gauss-Seidel solver
m_gaussSeidelSolver.solve(result->getMlcpProblem(), &(result->getMlcpSolution()));
// lambda
const Eigen::VectorXd& lambda = result->getMlcpSolution().x;
if (lambda.size() == 0)
{
return result;
}
// Copy constraintForces back to contact
auto& constraintsMapping = result->getConstraintsMapping();
const auto& activeConstraints = result->getActiveConstraints();
for (auto& constraint : activeConstraints)
{
if (constraint->getType() != ConstraintType::FRICTIONLESS_3DCONTACT)
{
continue;
}
ptrdiff_t indexConstraint = constraintsMapping.getValue(constraint.get());
SURGSIM_ASSERT(indexConstraint >= 0) << "Index for constraint is invalid: " << indexConstraint << std::endl;
auto contactConstraintData = std::dynamic_pointer_cast<ContactConstraintData>(constraint->getData());
SURGSIM_ASSERT(contactConstraintData != nullptr) << "";
auto contact = contactConstraintData->getContact();
contact->force = lambda[indexConstraint] * contact->normal;
}
return result;
}
void SolveMlcp::setMaxIterations(size_t maxIterations)
{
m_gaussSeidelSolver.setMaxIterations(maxIterations);
}
size_t SolveMlcp::getMaxIterations() const
{
return m_gaussSeidelSolver.getMaxIterations();
}
void SolveMlcp::setPrecision(double epsilon)
{
m_gaussSeidelSolver.setEpsilonConvergence(epsilon);
}
double SolveMlcp::getPrecision() const
{
return m_gaussSeidelSolver.getEpsilonConvergence();
}
void SolveMlcp::setContactTolerance(double epsilon)
{
m_gaussSeidelSolver.setContactTolerance(epsilon);
}
double SolveMlcp::getContactTolerance() const
{
return m_gaussSeidelSolver.getContactTolerance();
}
}; // Physics
}; // SurgSim
|