MlcpPhysicsProblem-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_INL_H
17 #define SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_INL_H
18 
19 #include "SurgSim/Math/Vector.h"
20 
21 namespace SurgSim
22 {
23 namespace Physics
24 {
25 
26 template <typename SubCDerivedType>
28  const Eigen::SparseVector<double>& newSubH,
29  const Eigen::MatrixBase<SubCDerivedType>& subC,
30  size_t indexSubC,
31  size_t indexNewSubH)
32 {
34 
35  // Update H, CHt, and HCHt with newSubH, denoted H'.
36  //
37  // Note that updates are linear for H and CHt, but not for HCHt:
38  // (H+H') = H+H'
39  // => H += H';
40  //
41  // C(H+H')t = CHt + CH't
42  // => CHt += CH't;
43  //
44  // (H+H')C(H+H')t = HCHt + HCH't + H'C(H+H')t
45  // => HCHt += H(CH't) + H'[C(H+H')t];
46 
47  Vector newCHt = subC * newSubH;
48  A.col(indexNewSubH) += H.middleCols(indexSubC, subC.rows()) * newCHt;
49  H.block(indexNewSubH, indexSubC, 1, subC.rows()) += newSubH.transpose();
50  CHt.block(indexSubC, indexNewSubH, subC.rows(), 1) += newCHt;
51  A.row(indexNewSubH) += newSubH.transpose() * CHt.middleRows(indexSubC, subC.rows());
52 }
53 
54 }
55 }
56 
57 #endif // SURGSIM_PHYSICS_MLCPPHYSICSPROBLEM_INL_H
Definition: DriveElementFromInputBehavior.cpp:27
Matrix CHt
The matrix , which is a matrix of size that is used to convert the vector of constraint forces to t...
Definition: MlcpPhysicsProblem.h:60
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: MlcpProblem.h:63
Matrix A
Matrix used to describe the mixed LCP problem.
Definition: MlcpProblem.h:66
Matrix H
The matrix , which is a matrix of size that converts from the degrees of freedom in the system (i...
Definition: MlcpPhysicsProblem.h:56
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:67
Definitions of small fixed-size vector types.
void updateConstraint(const Eigen::SparseVector< double > &newSubH, const Eigen::MatrixBase< SubCDerivedType > &subC, size_t indexSubC, size_t indexNewSubH)
Applies a new constraint to a specific Representation.
Definition: MlcpPhysicsProblem-inl.h:27