DART  6.10.1
PGSLCPSolver.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_CONSTRAINT_PGSLCPSOLVER_HPP_
34 #define DART_CONSTRAINT_PGSLCPSOLVER_HPP_
35 
36 #include <cstddef>
37 
38 #include "dart/config.hpp"
40 
41 namespace dart {
42 namespace constraint {
43 
48 class PGSLCPSolver : public LCPSolver
49 {
50 public:
52  explicit PGSLCPSolver(double _timestep);
53 
55  virtual ~PGSLCPSolver();
56 
57  // Documentation inherited
58  void solve(ConstrainedGroup* _group) override;
59 
60 #ifndef NDEBUG
61 private:
63  bool isSymmetric(std::size_t _n, double* _A);
64 
66  bool isSymmetric(
67  std::size_t _n, double* _A, std::size_t _begin, std::size_t _end);
68 
70  void print(
71  std::size_t _n,
72  double* _A,
73  double* _x,
74  double* _lo,
75  double* _hi,
76  double* _b,
77  double* w,
78  int* _findex);
79 #endif
80 };
81 
82 struct PGSOption
83 {
84  int itermax;
85  double sor_w;
86  double eps_ea;
87  double eps_res;
88  double eps_div;
89 
90  void setDefault();
91 };
92 
93 bool solvePGS(
94  int n,
95  int nskip,
96  int /*nub*/,
97  double* A,
98  double* x,
99  double* b,
100  double* lo,
101  double* hi,
102  int* findex,
103  PGSOption* option);
104 
105 } // namespace constraint
106 } // namespace dart
107 
108 #endif // DART_CONSTRAINT_PGSLCPSOLVER_HPP_
const CollisionOption & option
Collision option of DART.
Definition: FCLCollisionDetector.cpp:157
ConstrainedGroup is a group of skeletons that interact each other with constraints.
Definition: ConstrainedGroup.hpp:58
Definition: LCPSolver.hpp:45
Definition: PGSLCPSolver.hpp:49
PGSLCPSolver(double _timestep)
Constructor.
Definition: PGSLCPSolver.cpp:51
void solve(ConstrainedGroup *_group) override
Solve constriant impulses for a constrained group.
Definition: PGSLCPSolver.cpp:61
void print(std::size_t _n, double *_A, double *_x, double *_lo, double *_hi, double *_b, double *w, int *_findex)
Print debug information.
Definition: PGSLCPSolver.cpp:259
virtual ~PGSLCPSolver()
Constructor.
Definition: PGSLCPSolver.cpp:56
bool isSymmetric(std::size_t _n, double *_A)
Return true if the matrix is symmetric.
Definition: PGSLCPSolver.cpp:194
bool solvePGS(int n, int nskip, int, double *A, double *x, double *b, double *lo, double *hi, int *findex, PGSOption *option)
Definition: PGSLCPSolver.cpp:355
Definition: BulletCollisionDetector.cpp:65
Definition: PGSLCPSolver.hpp:83
int itermax
Definition: PGSLCPSolver.hpp:84
void setDefault()
Definition: PGSLCPSolver.cpp:533
double eps_div
Definition: PGSLCPSolver.hpp:88
double sor_w
Definition: PGSLCPSolver.hpp:85
double eps_res
Definition: PGSLCPSolver.hpp:87
double eps_ea
Definition: PGSLCPSolver.hpp:86