All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ContactLCP.h
Go to the documentation of this file.
1 //=================================================================================================
20 //=================================================================================================
21 
22 #ifndef _BLAZE_MATH_PROBLEMS_CONTACTLCP_H_
23 #define _BLAZE_MATH_PROBLEMS_CONTACTLCP_H_
24 
25 
26 //*************************************************************************************************
27 // Includes
28 //*************************************************************************************************
29 
30 #include <cmath>
33 #include <blaze/math/Functions.h>
34 #include <blaze/math/Infinity.h>
35 #include <blaze/system/Precision.h>
36 #include <blaze/util/Types.h>
37 
38 
39 namespace blaze {
40 
41 //=================================================================================================
42 //
43 // CLASS DEFINITION
44 //
45 //=================================================================================================
46 
47 //*************************************************************************************************
53 struct ContactLCP
54 {
55  //**Utility functions***************************************************************************
58  inline size_t size () const;
59  inline void project ( size_t index );
60  inline real lbound ( size_t index ) const;
61  inline real ubound ( size_t index ) const;
62  inline real residual( size_t index ) const;
63  inline real residual() const;
65  //**********************************************************************************************
66 
67  //**Member variables****************************************************************************
71  VecN b_;
72  VecN x_;
74 
75  //**********************************************************************************************
76 };
77 //*************************************************************************************************
78 
79 
80 
81 
82 //=================================================================================================
83 //
84 // UTILITY FUNCTIONS
85 //
86 //=================================================================================================
87 
88 //*************************************************************************************************
93 inline size_t ContactLCP::size() const
94 {
95  return x_.size();
96 }
97 //*************************************************************************************************
98 
99 
100 //*************************************************************************************************
106 inline void ContactLCP::project( size_t index )
107 {
108  x_[index] = min( ubound( index ), max( lbound( index ), x_[index] ) );
109 }
110 //*************************************************************************************************
111 
112 
113 //*************************************************************************************************
119 inline real ContactLCP::lbound( size_t index ) const
120 {
121  return ( index%3 != 0 ) ? ( -cof_[index/3] * x_[index - index%3] ) : ( real(0) );
122 }
123 //*************************************************************************************************
124 
125 
126 //*************************************************************************************************
132 inline real ContactLCP::ubound( size_t index ) const
133 {
134  return ( index%3 != 0 ) ? ( cof_[index/3] * x_[index - index%3] ) : ( inf );
135 }
136 //*************************************************************************************************
137 
138 
139 //*************************************************************************************************
145 inline real ContactLCP::residual( size_t index ) const
146 {
147  return max( x_[index] - ubound( index ),
148  min( x_[index] - lbound( index ), ( A_ * x_ )[index] + b_[index] ) );
149 }
150 //*************************************************************************************************
151 
152 
153 //*************************************************************************************************
159 {
160  real rmax( 0 );
161 
162  for( size_t i=0; i<A_.rows(); ++i )
163  rmax = max( rmax, std::fabs( residual( i ) ) );
164 
165  return rmax;
166 }
167 //*************************************************************************************************
168 
169 } // namespace blaze
170 
171 #endif