blob: 29d0e6632f75cffed828705459931ce2ecfe27cd (
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
|
//
// 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];
const Friction& friction = 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) * constraints[0].m_direction;
if (friction.m_direction.norm() > SIMD_EPSILON)
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
}
else if (constraints.size() == 2)
{
// TODO : friction
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
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 Friction& friction = 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) * constraints[0].m_direction;
x[i] += constraints[0].m_value * constraints[0].m_direction;
if (friction.m_direction.norm() > SIMD_EPSILON)
{
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
x[i] += friction.m_dv;
}
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value;
}
else
x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction;
}
}
// update the constraints
virtual void update(const TVStack& dv, const TVStack& backupVelocity);
virtual void setConstraintDirections();
};
#endif /* btContactProjection_h */
|
