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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
|
/*
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
*
* 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 JACOBIAN_ENTRY_H
#define JACOBIAN_ENTRY_H
#include "SimdVector3.h"
#include "Dynamics/RigidBody.h"
//notes:
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the JacobianEntry memory layout 16 bytes
// if you only are interested in angular part, just feed massInvA and massInvB zero
/// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error
class JacobianEntry
{
public:
JacobianEntry() {};
//constraint between two different rigidbodies
JacobianEntry(
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA,
const SimdVector3& inertiaInvB,
const SimdScalar massInvB)
:m_jointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_jointAxis));
m_bJ = world2B*(rel_pos2.cross(m_jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
}
//angular constraint between two different rigidbodies
JacobianEntry(const SimdVector3& jointAxis,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& inertiaInvA,
const SimdVector3& inertiaInvB)
:m_jointAxis(m_jointAxis)
{
m_aJ= world2A*m_jointAxis;
m_bJ = world2B*-m_jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
}
//constraint on one rigidbody
JacobianEntry(
const SimdMatrix3x3& world2A,
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA)
:m_jointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(m_jointAxis));
m_bJ = world2A*(rel_pos2.cross(m_jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = SimdVector3(0.f,0.f,0.f);
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
}
SimdScalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
{
const JacobianEntry& jacA = *this;
SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis);
SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
{
const JacobianEntry& jacA = *this;
SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis;
SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
SimdVector3 lin0 = massInvA * lin ;
SimdVector3 lin1 = massInvB * lin;
SimdVector3 sum = ang0+ang1+lin0+lin1;
return sum[0]+sum[1]+sum[2];
}
SimdScalar getRelativeVelocity(const SimdVector3& linvelA,const SimdVector3& angvelA,const SimdVector3& linvelB,const SimdVector3& angvelB)
{
SimdVector3 linrel = linvelA - linvelB;
SimdVector3 angvela = angvelA * m_aJ;
SimdVector3 angvelb = angvelB * m_bJ;
linrel *= m_jointAxis;
angvela += angvelb;
angvela += linrel;
SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
return rel_vel2 + SIMD_EPSILON;
}
//private:
SimdVector3 m_jointAxis;
SimdVector3 m_aJ;
SimdVector3 m_bJ;
SimdVector3 m_0MinvJt;
SimdVector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
SimdScalar m_Adiag;
};
#endif //JACOBIAN_ENTRY_H
|
