-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathpbd_base_constraints.cpp
227 lines (190 loc) · 9.33 KB
/
pbd_base_constraints.cpp
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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
#include "pbd_base_constraints.h"
#include "physics_util.h"
#define USE_QUATERNIONS_LINEARIZED_FORMULAS
void calculate_positional_constraint_preprocessed_data(Entity* e1, Entity* e2, vec3 r1_lc, vec3 r2_lc, Position_Constraint_Preprocessed_Data* pcpd) {
pcpd->e1 = e1;
pcpd->e2 = e2;
pcpd->r1_wc = quaternion_apply_to_vec3(&e1->world_rotation, r1_lc);
pcpd->r2_wc = quaternion_apply_to_vec3(&e2->world_rotation, r2_lc);
pcpd->e1_inverse_inertia_tensor = get_dynamic_inverse_inertia_tensor(e1);
pcpd->e2_inverse_inertia_tensor = get_dynamic_inverse_inertia_tensor(e2);
}
r64 positional_constraint_get_delta_lambda(Position_Constraint_Preprocessed_Data* pcpd, r64 h, r64 compliance, r64 lambda, vec3 delta_x) {
r64 c = gm_vec3_length(delta_x);
// We need to avoid calculations when delta_x is zero or very very close to zero, otherwise we will might run into
// big problems because of floating-point precision
const r64 EPSILON = 1e-50;
if (c <= EPSILON) {
return 0.0;
}
Entity* e1 = pcpd->e1;
Entity* e2 = pcpd->e2;
vec3 r1_wc = pcpd->r1_wc;
vec3 r2_wc = pcpd->r2_wc;
mat3 e1_inverse_inertia_tensor = pcpd->e1_inverse_inertia_tensor;
mat3 e2_inverse_inertia_tensor = pcpd->e2_inverse_inertia_tensor;
vec3 n = (vec3) {delta_x.x / c, delta_x.y / c, delta_x.z / c};
// calculate the inverse masses of both entities
r64 w1 = e1->inverse_mass + gm_vec3_dot(gm_vec3_cross(r1_wc, n), gm_mat3_multiply_vec3(&e1_inverse_inertia_tensor, gm_vec3_cross(r1_wc, n)));
r64 w2 = e2->inverse_mass + gm_vec3_dot(gm_vec3_cross(r2_wc, n), gm_mat3_multiply_vec3(&e2_inverse_inertia_tensor, gm_vec3_cross(r2_wc, n)));
assert(w1 + w2 != 0.0);
// calculate the delta_lambda (XPBD) and updates the constraint
r64 til_compliance = compliance / (h * h);
r64 delta_lambda = (- c - til_compliance * lambda) / (w1 + w2 + til_compliance);
return delta_lambda;
}
// Apply the positional constraint, updating the position and orientation of the entities accordingly
void positional_constraint_apply(Position_Constraint_Preprocessed_Data* pcpd, r64 delta_lambda, vec3 delta_x) {
r64 c = gm_vec3_length(delta_x);
// We need to avoid calculations when delta_x is zero or very very close to zero, otherwise we will might run into
// big problems because of floating-point precision
const r64 EPSILON = 1e-50;
if (c <= EPSILON) {
return;
}
Entity* e1 = pcpd->e1;
Entity* e2 = pcpd->e2;
vec3 r1_wc = pcpd->r1_wc;
vec3 r2_wc = pcpd->r2_wc;
mat3 e1_inverse_inertia_tensor = pcpd->e1_inverse_inertia_tensor;
mat3 e2_inverse_inertia_tensor = pcpd->e2_inverse_inertia_tensor;
vec3 n = (vec3) {delta_x.x / c, delta_x.y / c, delta_x.z / c};
// calculates the positional impulse
vec3 positional_impulse = gm_vec3_scalar_product(delta_lambda, n);
// updates the position of the entities based on eq (6) and (7)
if (!e1->fixed) {
e1->world_position = gm_vec3_add(e1->world_position, gm_vec3_scalar_product(e1->inverse_mass, positional_impulse));
}
if (!e2->fixed) {
e2->world_position = gm_vec3_add(e2->world_position, gm_vec3_scalar_product(-e2->inverse_mass, positional_impulse));
}
// updates the rotation of the entities based on eq (8) and (9)
vec3 aux1 = gm_mat3_multiply_vec3(&e1_inverse_inertia_tensor, gm_vec3_cross(r1_wc, positional_impulse));
vec3 aux2 = gm_mat3_multiply_vec3(&e2_inverse_inertia_tensor, gm_vec3_cross(r2_wc, positional_impulse));
#ifdef USE_QUATERNIONS_LINEARIZED_FORMULAS
Quaternion aux_q1 = (Quaternion){aux1.x, aux1.y, aux1.z, 0.0};
Quaternion aux_q2 = (Quaternion){aux2.x, aux2.y, aux2.z, 0.0};
Quaternion q1 = quaternion_product(&aux_q1, &e1->world_rotation);
Quaternion q2 = quaternion_product(&aux_q2, &e2->world_rotation);
if (!e1->fixed) {
e1->world_rotation.x = e1->world_rotation.x + 0.5 * q1.x;
e1->world_rotation.y = e1->world_rotation.y + 0.5 * q1.y;
e1->world_rotation.z = e1->world_rotation.z + 0.5 * q1.z;
e1->world_rotation.w = e1->world_rotation.w + 0.5 * q1.w;
// should we normalize?
e1->world_rotation = quaternion_normalize(&e1->world_rotation);
}
if (!e2->fixed) {
e2->world_rotation.x = e2->world_rotation.x - 0.5 * q2.x;
e2->world_rotation.y = e2->world_rotation.y - 0.5 * q2.y;
e2->world_rotation.z = e2->world_rotation.z - 0.5 * q2.z;
e2->world_rotation.w = e2->world_rotation.w - 0.5 * q2.w;
// should we normalize?
e2->world_rotation = quaternion_normalize(&e2->world_rotation);
}
#else
if (!e1->fixed) {
r64 e1_rotation_angle = gm_vec3_length(aux1);
vec3 e1_rotation_axis = gm_vec3_normalize(aux1);
Quaternion e1_orientation_change = quaternion_new_radians(e1_rotation_axis, e1_rotation_angle);
e1->world_rotation = quaternion_product(&e1_orientation_change, &e1->world_rotation);
// should we normalize?
e1->world_rotation = quaternion_normalize(&e1->world_rotation);
}
if (!e2->fixed) {
r64 e2_rotation_angle = -gm_vec3_length(aux2);
vec3 e2_rotation_axis = gm_vec3_normalize(aux2);
Quaternion e2_orientation_change = quaternion_new_radians(e2_rotation_axis, e2_rotation_angle);
e2->world_rotation = quaternion_product(&e2_orientation_change, &e2->world_rotation);
// should we normalize?
e2->world_rotation = quaternion_normalize(&e2->world_rotation);
}
#endif
}
void calculate_angular_constraint_preprocessed_data(Entity* e1, Entity* e2, Angular_Constraint_Preprocessed_Data* acpd) {
acpd->e1 = e1;
acpd->e2 = e2;
acpd->e1_inverse_inertia_tensor = get_dynamic_inverse_inertia_tensor(e1);
acpd->e2_inverse_inertia_tensor = get_dynamic_inverse_inertia_tensor(e2);
}
r64 angular_constraint_get_delta_lambda(Angular_Constraint_Preprocessed_Data* acpd, r64 h, r64 compliance, r64 lambda, vec3 delta_q) {
r64 theta = gm_vec3_length(delta_q);
// We need to avoid calculations when delta_q is zero or very very close to zero, otherwise we will might run into
// big problems because of floating-point precision
const r64 EPSILON = 1e-50;
if (theta <= EPSILON) {
return 0.0;
}
Entity* e1 = acpd->e1;
Entity* e2 = acpd->e2;
mat3 e1_inverse_inertia_tensor = acpd->e1_inverse_inertia_tensor;
mat3 e2_inverse_inertia_tensor = acpd->e2_inverse_inertia_tensor;
vec3 n = (vec3) {delta_q.x / theta, delta_q.y / theta, delta_q.z / theta};
// calculate the inverse masses of both entities
r64 w1 = gm_vec3_dot(n, gm_mat3_multiply_vec3(&e1_inverse_inertia_tensor, n));
r64 w2 = gm_vec3_dot(n, gm_mat3_multiply_vec3(&e2_inverse_inertia_tensor, n));
assert(w1 + w2 != 0.0);
// calculate the delta_lambda (XPBD) and updates the constraint
r64 til_compliance = compliance / (h * h);
r64 delta_lambda = (- theta - til_compliance * lambda) / (w1 + w2 + til_compliance);
return delta_lambda;
}
// Apply the angular constraint, updating the orientation of the entities accordingly
void angular_constraint_apply(Angular_Constraint_Preprocessed_Data* acpd, r64 delta_lambda, vec3 delta_q) {
r64 theta = gm_vec3_length(delta_q);
// We need to avoid calculations when delta_q is zero or very very close to zero, otherwise we will might run into
// big problems because of floating-point precision
const r64 EPSILON = 1e-50;
if (theta <= EPSILON) {
return;
}
Entity* e1 = acpd->e1;
Entity* e2 = acpd->e2;
mat3 e1_inverse_inertia_tensor = acpd->e1_inverse_inertia_tensor;
mat3 e2_inverse_inertia_tensor = acpd->e2_inverse_inertia_tensor;
vec3 n = (vec3) {delta_q.x / theta, delta_q.y / theta, delta_q.z / theta};
// calculates the positional impulse
vec3 positional_impulse = gm_vec3_scalar_product(-delta_lambda, n);
// updates the rotation of the entities based on eq (8) and (9)
vec3 aux1 = gm_mat3_multiply_vec3(&e1_inverse_inertia_tensor, positional_impulse);
vec3 aux2 = gm_mat3_multiply_vec3(&e2_inverse_inertia_tensor, positional_impulse);
#ifdef USE_QUATERNIONS_LINEARIZED_FORMULAS
Quaternion aux_q1 = (Quaternion){aux1.x, aux1.y, aux1.z, 0.0};
Quaternion aux_q2 = (Quaternion){aux2.x, aux2.y, aux2.z, 0.0};
Quaternion q1 = quaternion_product(&aux_q1, &e1->world_rotation);
Quaternion q2 = quaternion_product(&aux_q2, &e2->world_rotation);
if (!e1->fixed) {
e1->world_rotation.x = e1->world_rotation.x + 0.5 * q1.x;
e1->world_rotation.y = e1->world_rotation.y + 0.5 * q1.y;
e1->world_rotation.z = e1->world_rotation.z + 0.5 * q1.z;
e1->world_rotation.w = e1->world_rotation.w + 0.5 * q1.w;
// should we normalize?
e1->world_rotation = quaternion_normalize(&e1->world_rotation);
}
if (!e2->fixed) {
e2->world_rotation.x = e2->world_rotation.x - 0.5 * q2.x;
e2->world_rotation.y = e2->world_rotation.y - 0.5 * q2.y;
e2->world_rotation.z = e2->world_rotation.z - 0.5 * q2.z;
e2->world_rotation.w = e2->world_rotation.w - 0.5 * q2.w;
// should we normalize?
e2->world_rotation = quaternion_normalize(&e2->world_rotation);
}
#else
if (!e1->fixed) {
r64 e1_rotation_angle = gm_vec3_length(aux1);
vec3 e1_rotation_axis = gm_vec3_normalize(aux1);
Quaternion e1_orientation_change = quaternion_new_radians(e1_rotation_axis, e1_rotation_angle);
e1->world_rotation = quaternion_product(&e1_orientation_change, &e1->world_rotation);
// should we normalize?
e1->world_rotation = quaternion_normalize(&e1->world_rotation);
}
if (!e2->fixed) {
r64 e2_rotation_angle = -gm_vec3_length(aux2);
vec3 e2_rotation_axis = gm_vec3_normalize(aux2);
Quaternion e2_orientation_change = quaternion_new_radians(e2_rotation_axis, e2_rotation_angle);
e2->world_rotation = quaternion_product(&e2_orientation_change, &e2->world_rotation);
// should we normalize?
e2->world_rotation = quaternion_normalize(&e2->world_rotation);
}
#endif
}