-
Notifications
You must be signed in to change notification settings - Fork 7
/
phase1.cpp
108 lines (94 loc) · 2.72 KB
/
phase1.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
//phase1
//It contains the logic for the phase 1
#define PHASE1_MODERATION
bool phase1_taking; //true if we are spinning on the item
int phase1_last_item; //The last item we were trying to pick up
int phase1_prefered_item;
bool phase1_collision;
float phase1_initial_att[3];
void phase1_init() {
phase1_taking = false;
phase1_last_item = -1;
#ifdef PHASE1_MODERATION
phase1_collision = true;
phase1_prefered_item = (blue_sphere) ? 0 : 1;
#else
phase1_collision = false;
phase1_prefered_item = (blue_sphere) ? 1 : 0;
#endif
}
void phase1_loop() {
if(!phase1_collision) { //If we haven't collided yet
phase1_collision = game.wasCollisionActive(); //We check if we have collided
if(phase1_collision) {
//If we have collided we change our target item
phase1_prefered_item = (phase1_prefered_item == 1) ? 0 : 1;
}
}
if(seconds < 80 || phase1_taking) {
//If there is plenty of time or if we are currently spinning we take the items
if(phase1_take(phase1_prefered_item, false) && (phase1_collision || phase1_take((phase1_prefered_item == 1) ? 0 : 1, true))) {
phase2_prepare();
}
} else {
phase2_prepare();
}
}
//phase1_take
//Tries to pick up a item
//target_item -> Item number
//return -> true if the item has already been picked up, false otherwise
bool phase1_take(int target_item, bool direct) {
if(is_item_collected[target_item]) {
//If the item has been picked up we return true
phase1_taking = false;
return true;
}
float zero[3] = {0.0f};
phase1_last_item = target_item;
#ifdef DEBUG_ACTIVE
DEBUG(("phase1:Going to item %i\n", target_item));
#endif
if(movement_moveto(item_position[target_item], direct) || phase1_taking) {
//If we have arrived or if we are currenly there spinning
float dot;
float angle;
if(!phase1_taking) {
//If we weren't spinning we record our current attitude to measure our rotation
phase1_taking = true;
int i;
for(i = 0; i < 3; ++i) {
phase1_initial_att[i] = our_state[ATT+i];
}
}
api.setPositionTarget(item_position[target_item]);
api.setVelocityTarget(zero);
dot = mathVecInner(phase1_initial_att, &our_state[ATT], 3);
//angle = acosf(dot);
angle = PI/2.0f - dot - (dot*dot*dot)/6.0f; //Taylor expansion of acos
api.setVelocityTarget(zero);
//zero = att_rate
if(!is_item_collected[target_item]) {
if(angle > PI/2) {
//zero[POS_Z] = 0.0f;
phase1_taking = false;
#ifdef DEBUG_ACTIVE
DEBUG(("phase1_take:Fail!"));
#endif
} else {
if(angle < 3*PI/8) {
zero[POS_Z] = MAX_ITEM_RATE/2;
} else {
zero[POS_Z] = 0.1f;
}
}
}/* else {
zero[POS_Z] = 0.0f;
}*/
api.setPositionTarget(item_position[target_item]);
api.setAttRateTarget(zero);
} else {
phase1_taking = false;
}
return false;
}