-
Notifications
You must be signed in to change notification settings - Fork 0
/
Constraints.h
64 lines (52 loc) · 1.84 KB
/
Constraints.h
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
#pragma once
#include "Particle.h"
#include "Defines.h"
#include "SimulationObjects.h"
#include "debug.h"
extern ContactObject* simulationGetFreeContact();
void rodConstraint(ConstraintObject& co) {
//Serial.print("constraint");
Vector3D posDelta = co.m_obj1->m_position - co.m_obj2->m_position;
FixedPoint currentLength = posDelta.magnitude();
debug("Running RodConstraint of length ",DEBUG_CONSTRAINT);
debug(TO_STRING(co.m_rodData.m_length), DEBUG_CONSTRAINT);
debug(" and current length ",DEBUG_CONSTRAINT);
debugln(TO_STRING(currentLength), DEBUG_CONSTRAINT);
//co.m_obj1->print();
//co.m_obj2->print();
posDelta.normalize();
//if( currentLength < co.m_rodData.m_length - EPSILON ) {
//debug("Rod too short by ",DEBUG_CONSTRAINT);
FixedPoint delta = co.m_rodData.m_length - currentLength;
debugln(TO_STRING(delta), DEBUG_CONSTRAINT);
ContactObject* contact = simulationGetFreeContact();
contact->m_c1 = co.m_obj1;
contact->m_c2 = co.m_obj2;
contact->m_restitution = 0;
contact->m_contactNormal = posDelta;
contact->m_penetration = delta;
//}
//else if( currentLength > co.m_rodData.m_length + EPSILON) {
//debugln("Rod too long by ",DEBUG_CONSTRAINT);
delta = currentLength - co.m_rodData.m_length;
//debugln(TO_STRING(delta), DEBUG_CONSTRAINT);
contact = simulationGetFreeContact();
contact->m_c1 = co.m_obj1;
contact->m_c2 = co.m_obj2;
contact->m_restitution = 0;
contact->m_contactNormal = posDelta*-ONE;
contact->m_penetration = delta;
//}
//else {
// debugln("Rod fine",DEBUG_CONSTRAINT);
//}
}
void buildRodConstraint(ConstraintObject *co, Object* obj1, Object* obj2, FixedPoint rodLength) {
Serial.println("New constraint:");
obj1->print();
obj2->print();
co->m_obj1 = obj1;
co->m_obj2 = obj2;
co->m_rodData.m_length = rodLength;
co->m_generator = rodConstraint;
}