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
|
/*
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erwin Coumans makes no representations about the suitability
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#ifndef SOLVE_2LINEAR_CONSTRAINT_H
#define SOLVE_2LINEAR_CONSTRAINT_H
#include "SimdMatrix3x3.h"
#include "SimdVector3.h"
class RigidBody;
/// constraint class used for lateral tyre friction.
class Solve2LinearConstraint
{
SimdScalar m_tau;
SimdScalar m_damping;
public:
Solve2LinearConstraint(SimdScalar tau,SimdScalar damping)
{
m_tau = tau;
m_damping = damping;
}
//
// solve unilateral constraint (equality, direct method)
//
void resolveUnilateralPairConstraint(
RigidBody* body0,
RigidBody* body1,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1);
//
// solving 2x2 lcp problem (inequality, direct solution )
//
void resolveBilateralPairConstraint(
RigidBody* body0,
RigidBody* body1,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1);
void resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdMatrix3x3& invInertiaBWS,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1);
};
#endif //SOLVE_2LINEAR_CONSTRAINT_H
|
