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
|
// Copyright 2006-2007 Nanorex, Inc. See LICENSE file for details.
#include "simulator.h"
#include "rigid-ode.h"
#ifndef USE_ODE
static void
check_rigid_support(struct part *p)
{
struct rigidBody *rb;
if (p->num_rigidBodies < 1) {
return;
}
rb = &p->rigidBodies[0];
if (p->num_rigidBodies > 1 || rb->num_attachments > 0) {
ERROR("no rigid body support");
}
}
#endif
void
rigid_init(struct part *p)
{
#ifdef USE_ODE
rigid_ode_init(p);
#else
check_rigid_support(p);
#endif
}
void
rigid_destroy(struct part *p)
{
#ifdef USE_ODE
rigid_ode_destroy(p);
#else
check_rigid_support(p);
#endif
}
void
rigid_relative_to_absolute(struct part *p, int bodyIndex, struct xyz relative, struct xyz *absolute)
{
#ifdef USE_ODE
rigid_ode_relative_to_absolute(p, bodyIndex, relative, absolute);
#else
ERROR("no rigid body support");
#endif
}
void
rigid_apply_force_relative(struct part *p, int bodyIndex, struct xyz force_location_relative, struct xyz force_direction_absolute)
{
#ifdef USE_ODE
rigid_ode_apply_force_relative(p, bodyIndex, force_location_relative, force_direction_absolute);
#else
ERROR("no rigid body support");
#endif
}
void
rigid_forces(struct part *p, struct xyz *positions, struct xyz *forces)
{
int i;
int j;
int atomIndex;
double f;
struct rigidBody *rb;
struct xyz absolute;
struct xyz delta;
struct xyz force;
// XXX also need to compute thermal transfer to body.
for (i=0; i<p->num_rigidBodies; i++) {
rb = &p->rigidBodies[i];
for (j=0; j<rb->num_attachments; j++) {
rigid_relative_to_absolute(p, i, rb->attachmentLocations[j], &absolute);
atomIndex = rb->attachmentAtomIndices[j];
vsub2(delta, positions[atomIndex], absolute);
f = vdot(delta, delta) * 1; // XXX unit conversion, spring force constant
vmul2c(force, delta, f);
vsub(forces[atomIndex], force);
rigid_apply_force_relative(p, i, rb->attachmentLocations[j], force);
}
}
}
|
