blob: b52bb2d7202f9a41e696a552da703a79d7567d6b (
plain)
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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
|
//
// btContactProjection.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/4/19.
//
#ifndef BT_CONTACT_PROJECTION_H
#define BT_CONTACT_PROJECTION_H
#include "btCGProjection.h"
#include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <iostream>
class btContactProjection : public btCGProjection
{
public:
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: btCGProjection(softBodies, dt)
{
}
virtual ~btContactProjection()
{
}
// apply the constraints
virtual void operator()(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
Friction& friction= frictions[0];
bool has_static_constraint = false;
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false && !has_static_constraint)
{
x[i] += friction.m_direction[j] * friction.m_value[j];
}
}
}
else if (constraints.size() == 2)
{
// TODO : friction
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
else
x[i].setZero();
}
}
virtual void enforceConstraint(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
const Friction& friction= frictions[0];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_value[j];
}
}
}
else if (constraints.size() == 2)
{
// TODO: friction
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
else
{
x[i].setZero();
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
}
}
// update the constraints
virtual void update(const TVStack& dv, const TVStack& backupVelocity);
virtual void setConstraintDirections();
};
#endif /* btContactProjection_h */
|
