1 /*
2
3 Copyright (C) 2015-2018 Night Dive Studios, LLC.
4
5 This program is free software: you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation, either version 3 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17
18 */
19 // Pelvis.cc is a destroyed biped. R.I.P. Life is pain, and in addition this should make it
20 // less fun to move in the indoor games. You should probably use the Biped instead unless
21 // you are a Twinkie and thus more concerned with inventory systems than dynamic fun...
22
23 // Comment rescinded. Pelvis now fun. End of line.
24 // =================================================
25
26 // Seamus, Nov 2, 1993...
27 // ========================
28
29 #include <iostream>
30 ////#include <conio.h>
31 #include "edms_int.h" //This is the object type library. It is universal.
32 #include "idof.h"
33
34 // Super secret Church-Blackley Boundary Condition Descriptor (BCD)...
35 // ===================================================================
36 // extern "C" {
37
38 #include "ss_flet.h"
39
40 Q EDMS_CYBER_FLOW1X = 100;
41 Q EDMS_CYBER_FLOW2X = 200;
42 Q EDMS_CYBER_FLOW3X = 270;
43
44 int32_t EDMS_BCD = 0;
45 bool pelvis_is_climbing = false;
46 int32_t edms_ss_head_bcd_flags;
47
48 fix hacked_head_bob_1 = fix_make(1, 0);
49 fix hacked_head_bob_2 = fix_make(1, 0);
50
51 //}
52
53 #define EDMS_DIV_ZERO_TOLERANCE .0005
54
55 // For lean-o-meter...
56 // -------------------
57 static Q V_ceiling[3], V_floor[3], V_wall[3];
58
59 // State information and utilities...
60 // ==================================
61 extern EDMS_Argblock_Pointer A;
62 extern Q S[MAX_OBJ][7][4], I[MAX_OBJ][DOF_MAX];
63 extern int32_t no_no_not_me[MAX_OBJ];
64
65 // Functions...
66 // ============
67 extern void (*idof_functions[MAX_OBJ])(int32_t), (*equation_of_motion[MAX_OBJ][7])(int32_t);
68
69 // Callbacks themselves...
70 // -----------------------
71 extern void (*EDMS_wall_contact)(physics_handle caller);
72
73 static Q fix_one = 1., point_five = .5, two_pi = 6.283185;
74
75 // Just a thought...
76 // =================
77 static Q object0, object1, object2, object3, object4, // Howzat??
78 object5, object6, object7, object8, object9, object10, object11, object12, object13, object14, object15, object16,
79 object17, object18, object19, object20, object21, object22;
80
81 static Q sin_alpha = 0, cos_alpha = 0, sin_beta = 0, cos_beta = 0, sin_gamma = 0, cos_gamma = 0, lp_x = 0, lp_y = 0,
82 lp_z = 0, Fmxm = 0, Fmym = 0, Fmzm = 0, T_beta = 0, T_gamma = 0;
83
84 static Q io17 = 0, io18 = 0, io19 = 0;
85
86 static Q checker = 0, wall_check = 0;
87
88 // Global for head information...
89 // ==============================
90 Q head_delta[3], head_kappa[3], body_delta[3], body_kappa[3];
91
92 // Storage for running feel...
93 // ===========================
94 Q bob_arg = 0;
95
96 #pragma require_prototypes off
97
98 // Here are the internal degrees of freedom:
99 // =========================================
pelvis_idof(int32_t object)100 void pelvis_idof(int32_t object) {
101
102 // attemp to speed up something
103 Q *i_object = I[object];
104 Q temp_Q;
105
106 // To do the head motion, collisions, and climbing...
107 // --------------------------------------------------
108 void get_head_of_death(int32_t), get_body_of_death(int32_t), do_climbing(int32_t object);
109
110 // Call me instead of having special code everywhere...
111 // ====================================================
112 extern void shall_we_dance(int32_t object, Q &result0, Q &result1, Q &result2);
113
114 pelvis_is_climbing = false;
115
116 indoor_terrain(A[object][0][0], // Get the info...
117 A[object][1][0], A[object][2][0], i_object[22],
118 on2ph[object], TFD_FULL);
119
120 V_ceiling[0].fix_to(terrain_info.cx); // Put it in...
121 V_ceiling[1].fix_to(terrain_info.cy);
122 V_ceiling[2].fix_to(terrain_info.cz);
123
124 V_floor[0].fix_to(terrain_info.fx);
125 V_floor[1].fix_to(terrain_info.fy);
126 V_floor[2].fix_to(terrain_info.fz);
127
128 Q mag = i_object[18] * i_object[18] + i_object[19] * i_object[19];
129 if (mag < .1 && abs(V_floor[0]) < .05 * i_object[22] && abs(V_floor[1]) < .05 * i_object[22]) {
130 V_floor[1].val = 0; // Turns on SlopeStand(tm)...
131 V_floor[0].val = 0;
132 }
133
134 V_wall[0].fix_to(terrain_info.wx);
135 V_wall[1].fix_to(terrain_info.wy);
136 V_wall[2].fix_to(terrain_info.wz);
137
138 object0.val = V_wall[0].val + V_floor[0].val + V_ceiling[0].val; // V_raw...
139 object1.val = V_wall[1].val + V_floor[1].val + V_ceiling[1].val;
140 object2.val = V_wall[2].val + V_floor[2].val + V_ceiling[2].val;
141
142 // checker = sqrt(object0*object0 + object1*object1 + object2*object2);
143 checker.val = fix_sqrt(fix_mul(object0.val, object0.val) + fix_mul(object1.val, object1.val) +
144 fix_mul(object2.val, object2.val));
145
146 if (checker > EDMS_DIV_ZERO_TOLERANCE) { // To get primitive...
147 object3.val = fix_div(fix_one.val, checker.val);
148 object9.val = fix_make(1, 0); // Are we in the rub???
149 } else
150 checker.val = object9.val = 0;
151
152 if (i_object[10] == 2)
153 object9.val = fix_make(1, 0); // Cyberspace...
154
155 object4.val = fix_mul(object3.val, object0.val); // The primitive V_n...
156 object5.val = fix_mul(object3.val, object1.val);
157 object6.val = fix_mul(object3.val, object2.val);
158
159 object7.val = fix_mul(i_object[21].val,
160 (fix_mul(A[object][0][1].val, object4.val) // Delta_magnitude...
161 + fix_mul(A[object][1][1].val, object5.val) + fix_mul(A[object][2][1].val, object6.val)));
162
163 object8.val = i_object[20].val;
164
165 if (i_object[10] > 0) {
166 object7.val = fix_mul(object7.val, fix_make(2, 0));
167 object8.val = fix_mul(object8.val, fix_make(2, 0));
168 }
169
170 object4.val = fix_mul(object7.val, object4.val); // Delta...
171 object5.val = fix_mul(object7.val, object5.val);
172 object6.val = fix_mul(object7.val, object6.val);
173
174 // CONTROL...
175 // ==========
176
177 // Head motion for fucking hacking...
178 // ----------------------------------
179 Q x_ease = A[object][0][0] - S[object][0][0];
180 Q y_ease = A[object][1][0] - S[object][1][0];
181 Q bob_delta = sqrt(x_ease * x_ease + y_ease * y_ease);
182 Q bob_speed = sqrt(A[object][0][1] * A[object][0][1] + A[object][1][1] * A[object][1][1]);
183
184 bob_arg.val += fix_div(fix_mul(fix_make(5, 0), bob_delta.val), (bob_speed.val + fix_make(1, 0)));
185
186 if (bob_arg > two_pi)
187 bob_arg.val = bob_arg.val - two_pi.val;
188
189 Q bob_fac = bob_speed * abs(sin(bob_arg));
190
191 #define EDMS_HEAD_BOB_HEIGHT 2
192
193 if (bob_fac > EDMS_HEAD_BOB_HEIGHT)
194 bob_fac = EDMS_HEAD_BOB_HEIGHT;
195
196 bob_fac.val = fix_make(EDMS_HEAD_BOB_HEIGHT, 0) - fix_mul(0x09999, bob_fac.val); // 0x9999 = .6
197
198 if (i_object[10] > 0)
199 bob_fac = 1;
200
201 io18.val = fix_mul(i_object[18].val, bob_fac.val);
202 io19.val = fix_mul(i_object[19].val, bob_fac.val);
203 io17.val = i_object[17].val;
204
205 // Let's not power through the walls anymore...
206 // --------------------------------------------
207 io18 *= (V_wall[0].val == 0);
208 io19 *= (V_wall[1].val == 0);
209 io17 *= (V_ceiling[2].val == 0);
210 if ((V_floor[2].val == 0) && (io17.val > 0))
211 io17.val = 0;
212
213 // Cyberama...
214 // -----------
215 if ((object9.val == 0) && (io17.val >= 0) && (i_object[25].val > 0x08000)) // 0x08000 = .5
216 io18.val = io19.val = io17.val = 0;
217
218 // Here are collisions with other objects...
219 // =========================================
220 shall_we_dance(object, object10, object11, object12);
221
222 object10.val = fix_mul(object10.val, i_object[20].val); // More general than it was...
223 object11.val = fix_mul(object11.val, i_object[20].val);
224 object12.val = fix_mul(object12.val, i_object[20].val);
225
226 // Let's not power through the walls anymore...
227 // --------------------------------------------
228 // object10 *= ( (V_wall[0].val< 0x028f) && (V_wall[0].val>-0x028f)); // 0x028f = 0.01
229 if (!((V_wall[0].val < 0x028f) && (V_wall[0].val > -0x028f)))
230 object10.val = 0;
231 // object11 *= ( (V_wall[1].val<0x028f) && (V_wall[1].val>-0x028f));
232 if (!((V_wall[1].val < 0x028f) && (V_wall[1].val > -0x028f)))
233 object11.val = 0;
234
235 // Back to business...
236 // ===================
237 sincos(A[object][3][0], &sin_alpha, &cos_alpha); // Positive for local...
238 sincos(A[object][4][0], &sin_beta, &cos_beta);
239 sincos(A[object][5][0], &sin_gamma, &cos_gamma);
240
241 // The head...
242 // ===========
243 int32_t edms_ss_bcd_flags_save = ss_edms_bcd_flags; // Save off info for after head call...
244 int32_t edms_ss_param_save = ss_edms_bcd_param;
245
246 Q head_check_x = 0;
247 Q head_check_y = 0;
248
249 get_head_of_death(object);
250
251 if (terrain_info.wx == 0)
252 head_check_x = 1;
253 if (terrain_info.wy == 0)
254 head_check_y = 1;
255
256 // io18 *= head_check_x;
257 // io19 *= head_check_y;
258
259 edms_ss_head_bcd_flags = ss_edms_bcd_flags;
260 if (terrain_info.cz != 0 || head_kappa[2] != 0)
261 i_object[17] = io17 = 0;
262
263 // The Body...
264 // ===========
265 get_body_of_death(object);
266 // io18 *= ( body_kappa[0] == 0 );
267 // io19 *= ( body_kappa[1] == 0 );
268 if (body_kappa[0].val != 0)
269 io18.val = 0;
270 if (body_kappa[1].val != 0)
271 io19.val = 0;
272
273 ss_edms_bcd_flags = edms_ss_bcd_flags_save;
274 ss_edms_bcd_param = edms_ss_param_save;
275
276 // Do climbing...
277 // ==============
278 do_climbing(object);
279
280 // Fateful attempt(Jump)...
281 // ========================
282 // object18 = 800*(io17>0)*(object9>0)*( io17 - A[object][2][1] ); //Jump...
283 object18.val = 0;
284 if (io17.val > 0 && object9.val > 0)
285 object18.val = fix_mul(fix_make(800, 0), (io17.val - A[object][2][1].val));
286
287 // Jump jets...
288 // ------------
289 if ((io17 < 0) && (terrain_info.cz == 0))
290 object18 = 800 * (-io17 - A[object][2][1]); // Jump jets...
291
292 // Climbing overriden with repulsors...
293 // ====================================
294 if (ss_edms_bcd_flags & SS_BCD_REPUL_ON) {
295
296 // Get the speed...
297 Q repulsor_speed = 21;
298 if ((ss_edms_bcd_flags & SS_BCD_REPUL_SPD) == SS_BCD_REPUL_NORM)
299 repulsor_speed = 7;
300
301 // Assume we're going up, unless...
302 if ((ss_edms_bcd_flags & SS_BCD_REPUL_TYPE) == SS_BCD_REPUL_DOWN)
303 repulsor_speed *= -.5;
304
305 // The parameter should be the desired height....
306 Q repul_height;
307 repul_height.fix_to(ss_edms_bcd_param);
308
309 Q nearness_or_something = repul_height - A[object][2][0];
310 if (abs(nearness_or_something) <= .333)
311 repulsor_speed *= 3 * nearness_or_something;
312
313 io17 = repulsor_speed;
314 if ((abs(A[object][2][1] - i_object[17]) > .6 * i_object[17]) && (terrain_info.cz == 0) &&
315 (repulsor_speed >= 0))
316 io17 += 50 * i_object[17];
317
318 object18 = i_object[26] * ((io17 - A[object][2][1]) + i_object[25]);
319
320 if (abs(io18) < .01)
321 io18 = i_object[18] * (V_wall[0] == 0);
322 if (abs(io19) < .01)
323 io19 = i_object[19] * (V_wall[1] == 0);
324
325 object9 = 1;
326 }
327
328 // Do climbing...
329 // ==============
330 do_climbing(object);
331
332 // Crouch torso bend thang and boogie boogie boogie...
333 // ===================================================
334 if ((i_object[7] > 0.0) || (i_object[0] < i_object[6]))
335 i_object[0] = i_object[6] * (1 - .636 * abs(S[object][4][0])); // Crouch...
336 else
337 i_object[0] = i_object[6];
338
339 // Cyberspace for real...
340 // ======================
341 Q drug_addict0 = i_object[IDOF_PELVIS_ROLL_DRAG] * A[object][0][1];
342 Q drug_addict1 = i_object[IDOF_PELVIS_ROLL_DRAG] * A[object][1][1];
343 if (abs(io18) == 0 && i_object[10] > 0)
344 drug_addict0 *= .2; // Skateware drag reduction...
345 if (abs(io19) == 0 && i_object[10] > 0)
346 drug_addict1 *= .2;
347
348 // Pelvis specifics...
349 // ===================
350 object20 = object8 * object0 - object4 + head_kappa[0] - head_delta[0] + io18 + body_kappa[0] -
351 body_delta[0] // F_mxyz...
352 + object9 * (-drug_addict0) + object10;
353
354 object21 = object8 * object1 - object5 + head_kappa[1] - head_delta[1] + io19 + body_kappa[1] - body_delta[1] +
355 object9 * (-drug_addict1) + object11;
356
357 object22 = object8 * object2 - (object18 == 0) * object6 + head_kappa[2] - head_delta[2] + object18 +
358 body_kappa[2] - body_delta[2] + object9 * (-i_object[23] * A[object][2][1]) + object12;
359
360 // Damage control...
361 // =================
362 Q dam0 = object8 * object0 - object4 + head_kappa[0] - head_delta[0];
363 Q dam1 = object8 * object1 - object5 + head_kappa[1] - head_delta[1];
364 Q dam2 = object8 * object2 - (object18 == 0) * object6 + head_kappa[2] - head_delta[2];
365
366 i_object[14] = abs(dam0) + abs(dam1) + abs(dam2) - 2 * i_object[26] * i_object[25]; // Damage??
367 if (i_object[14] > 0)
368 i_object[14] *= i_object[IDOF_PELVIS_MASS_RECIP] * (io17 < .5);
369 else
370 i_object[14] = 0;
371
372 // Is there a projectile hit?
373 // ==========================
374 if (i_object[35] > 0) {
375
376 // Let's not power through the walls anymore...
377 // --------------------------------------------
378 i_object[32] *= ((V_wall[0] < 0.01) && (V_wall[0] > -0.01));
379 i_object[33] *= ((V_wall[1] < 0.01) && (V_wall[1] > -0.01));
380 i_object[34] *= ((V_ceiling[2] < 0.01) && (V_ceiling[2] > -0.01));
381
382 object20 += i_object[32];
383 object21 += i_object[33];
384 object22 += i_object[34];
385
386 i_object[35] = 0;
387 i_object[32] = 0;
388 i_object[33] = 0;
389 i_object[34] = 0;
390 }
391
392 Fmxm = object20 * cos_alpha + object21 * sin_alpha; // Locals...
393 Fmym = -object20 * sin_alpha + object21 * cos_alpha;
394
395 lp_z = -.1 * i_object[0] * cos_beta * cos_gamma;
396
397 Q Head_tau_beta = -.1 * i_object[0] * sin_beta * (head_kappa[2] - head_delta[2]),
398 Head_tau_gamma = -.1 * i_object[0] * sin_gamma * (head_kappa[2] - head_delta[2]);
399
400 if (((V_wall[1] != 0) && (head_check_y == 0)) || ((V_wall[0] != 0) && (head_check_x == 0)))
401 i_object[15] = 0;
402
403 T_beta = -(lp_z * Fmxm) + i_object[7] + Head_tau_beta; // Actual torques...
404 T_gamma = -(-Fmym * lp_z) + .04 * i_object[16] * +Head_tau_gamma;
405
406 // Kickbacks...
407 // ============
408 if (abs(i_object[8]) > 0) {
409 T_beta -= cos_alpha * i_object[8] + sin_alpha * i_object[9];
410 T_gamma = -sin_alpha * i_object[8] + cos_alpha * i_object[9];
411
412 object20 -= i_object[8]; // For zero g...
413 object21 -= i_object[9];
414
415 i_object[8] = i_object[9] = 0;
416 }
417
418 object17 = i_object[28] * (1 + 1.2 * (i_object[16] == 0)); // 3 is 2
419
420 // Angular play (citadel) ...
421 // ==========================
422 if (S[object][3][0] > two_pi)
423 S[object][3][0] -= two_pi;
424 if (S[object][3][0] < -two_pi)
425 S[object][3][0] += two_pi;
426
427 // Try the equations of motion here for grins...
428 // =============================================
429 S[object][0][2] = i_object[IDOF_PELVIS_MASS_RECIP] * (object20);
430 S[object][1][2] = i_object[IDOF_PELVIS_MASS_RECIP] * (object21);
431 S[object][2][2] = i_object[IDOF_PELVIS_MASS_RECIP] * (object22)-i_object[IDOF_PELVIS_GRAVITY];
432 S[object][3][2] = i_object[IDOF_PELVIS_MOI_RECIP] * (i_object[16] - object17 * A[object][3][1]);
433 S[object][4][2] = i_object[IDOF_PELVIS_MOI_RECIP] * (T_beta - 1.5 * i_object[1] * A[object][4][0] /**(1-.5*(i_object[10]==1))*/
434 - .8 * i_object[2] * A[object][4][1] /**(1-.5*(i_object[10]==1))*/);
435
436 S[object][5][2] = i_object[IDOF_PELVIS_MOI_RECIP] * (T_gamma - i_object[1] * A[object][5][0] /**(1-.5*(i_object[10]==1))*/
437 - .8 * i_object[2] * A[object][5][1] /**(1-.5*(i_object[10]==1))*/
438 + i_object[15]);
439
440 // That's all, folks...
441 // ====================
442 }
443
444 // Here we'll get the head information we all want so badly...
445 // ===========================================================
get_head_of_death(int32_t object)446 void get_head_of_death(int32_t object) {
447 Q *i_object = I[object];
448 Q vec0, vec1, vec2, test, mul, vv0, vv1, vv2, dmag, kmag;
449
450 Q offset_x = i_object[0] * sin(A[object][4][0]), offset_y = -1.5 * i_object[0] * sin(A[object][5][0]),
451 offset_z = i_object[0] * cos(A[object][4][0]) * cos(A[object][5][0]);
452
453 Q sin_alpha = 0, cos_alpha = 0;
454
455 Q final_x = 0, final_y = 0;
456
457 sincos(-A[object][3][0], &sin_alpha, &cos_alpha);
458 final_x = cos_alpha * offset_x + sin_alpha * offset_y;
459 final_y = -sin_alpha * offset_x + cos_alpha * offset_y;
460
461 indoor_terrain(A[object][0][0] + final_x, A[object][1][0] + final_y, A[object][2][0] + offset_z, .75 * i_object[22],
462 -1 /*on2ph[object]*/, TFD_FULL);
463
464 Q mag = i_object[18] * i_object[18] + i_object[19] * i_object[19];
465 if (mag < .1 && abs(V_floor[0]) < .05 * i_object[22] && abs(V_floor[1]) < .05 * i_object[22]) {
466 terrain_info.fx = terrain_info.fy = 0;
467 }
468
469 vec0.fix_to(terrain_info.fx + terrain_info.cx + terrain_info.wx);
470 vec1.fix_to(terrain_info.fy + terrain_info.cy + terrain_info.wy);
471 vec2.fix_to(terrain_info.fz + terrain_info.cz + terrain_info.wz);
472
473 test = sqrt(vec0 * vec0 + vec1 * vec1 + vec2 * vec2);
474
475 if (test > EDMS_DIV_ZERO_TOLERANCE)
476 mul = fix_one / test; // To get primitive...
477 else
478 test = mul = 0;
479
480 vv0 = mul * vec0; // The primitive V_n...
481 vv1 = mul * vec1;
482 vv2 = mul * vec2;
483
484 dmag = i_object[IDOF_PELVIS_D] * (A[object][0][1] * vv0 // Delta_magnitude...
485 + A[object][1][1] * vv1 + A[object][2][1] * vv2);
486
487 head_delta[0] = dmag * vv0; // Delta...
488 head_delta[1] = dmag * vv1;
489 head_delta[2] = dmag * vv2;
490
491 // if (test < .5*i_object[22]) kmag = i_object[20]; //Omega_magnitude...
492 // else kmag = i_object[20]/test;
493 kmag = i_object[IDOF_PELVIS_K];
494
495 head_kappa[0] = kmag * vec0;
496 head_kappa[1] = kmag * vec1;
497 head_kappa[2] = kmag * vec2;
498 }
499
get_body_of_death(int32_t object)500 void get_body_of_death(int32_t object) {
501 Q *i_object = I[object];
502
503 Q vec0, vec1, vec2, test, mul, vv0, vv1, vv2, dmag, kmag;
504
505 Q half_height = .5 * i_object[0];
506
507 Q offset_x = half_height * sin(A[object][4][0]), offset_y = -1.5 * half_height * sin(A[object][5][0]),
508 offset_z = half_height * cos(A[object][4][0]) * cos(A[object][5][0]);
509
510 Q sin_alpha = 0, cos_alpha = 0;
511
512 Q final_x = 0, final_y = 0;
513
514 sincos(-A[object][3][0], &sin_alpha, &cos_alpha);
515 final_x = cos_alpha * offset_x + sin_alpha * offset_y;
516 final_y = -sin_alpha * offset_x + cos_alpha * offset_y;
517
518 indoor_terrain(A[object][0][0] + final_x, A[object][1][0] + final_y, A[object][2][0] + offset_z, .33 * i_object[0],
519 -1 /*on2ph[object]*/, TFD_FULL);
520
521 // Zero result!
522 // ============
523 body_kappa[0] = body_kappa[1] = body_kappa[2] = 0;
524 body_delta[0] = body_delta[1] = body_delta[2] = 0;
525
526 // Do ANYTHING?
527 // ------------
528 Q abtotal = abs(terrain_info.fx) + abs(terrain_info.fy) + abs(terrain_info.fz);
529 abtotal += abs(terrain_info.wx) + abs(terrain_info.wy) + abs(terrain_info.wz);
530 abtotal += abs(terrain_info.cx) + abs(terrain_info.cy) + abs(terrain_info.cz);
531 if (abtotal != 0) {
532 Q mag = i_object[18] * i_object[18] + i_object[19] * i_object[19];
533 if (mag < .1 && abs(V_floor[0]) < .05 * i_object[22] && abs(V_floor[1]) < .05 * i_object[22])
534 terrain_info.fx = terrain_info.fy = 0;
535
536 vec0.fix_to(terrain_info.fx + terrain_info.cx + terrain_info.wx);
537 vec1.fix_to(terrain_info.fy + terrain_info.cy + terrain_info.wy);
538 vec2.fix_to(terrain_info.fz + terrain_info.cz + terrain_info.wz);
539
540 test = sqrt(vec0 * vec0 + vec1 * vec1 + vec2 * vec2);
541
542 if (test > EDMS_DIV_ZERO_TOLERANCE)
543 mul = fix_one / test; // To get primitive...
544 else
545 test = mul = 0;
546
547 vv0 = mul * vec0; // The primitive V_n...
548 vv1 = mul * vec1;
549 vv2 = mul * vec2;
550
551 vec2 = vv2 = 0;
552
553 dmag = i_object[IDOF_PELVIS_D] * (A[object][0][1] * vv0 // Delta_magnitude...
554 + A[object][1][1] * vv1 + A[object][2][1] * vv2);
555
556 body_delta[0] = dmag * vv0; // Delta...
557 body_delta[1] = dmag * vv1;
558 body_delta[2] = dmag * vv2;
559
560 kmag = i_object[20];
561
562 body_kappa[0] = kmag * vec0;
563 body_kappa[1] = kmag * vec1;
564 body_kappa[2] = kmag * vec2;
565
566 } // Do NOTHING...
567 }
568
569 // Climbing stuff also removed for speed of compilations...
570 // ========================================================
do_climbing(int32_t object)571 void do_climbing(int32_t object) {
572 Q *i_object = I[object];
573
574 // Hellishness...
575 // ==============
576 if ((i_object[17] > 0) &&
577 ((ss_edms_bcd_flags & SS_BCD_MISC_CLIMB) || (edms_ss_head_bcd_flags & SS_BCD_MISC_CLIMB))) {
578 Q ass = sqrt((.05 * i_object[18]) * i_object[18] + (.05 * i_object[19]) * i_object[19]);
579 Q ratio = i_object[18] * object0 + i_object[19] * object1;
580
581 if (ratio > 0)
582 ass = 0;
583
584 pelvis_is_climbing = true;
585
586 if (checker > 0) {
587 io17 = .02 * ass; // + 100*( .2*i_object[22] - V_[floor][2] );
588 if ((terrain_info.cz != 0))
589 io17 = 0;
590 io18 = -.4 * i_object[IDOF_PELVIS_RADIUS] * object0 * object8 / checker + .5 * i_object[18];
591 io19 = -.4 * i_object[IDOF_PELVIS_RADIUS] * object1 * object8 / checker + .5 * i_object[19];
592 i_object[16] *= .5;
593
594 // Set the mojo...
595 // ===============
596 object18 = 800 * (io17 > 0) * (io17 - A[object][2][1]);
597 }
598 }
599
600 // AutoClimbing(tm) is for wussies (is superseeded by climbing)...
601 // ===============================================================
602 else if ((ss_edms_bcd_flags & SS_BCD_MISC_STAIR) /*&& (i_object[17] == 0) (io17==0) */) {
603 if ((checker > 0) && (abs(i_object[18]) + abs(i_object[19]) > .01)) {
604 Q ratio = (i_object[18] + A[object][0][1]) * object0 + (i_object[19] + A[object][1][1]) * object1;
605
606 if (ratio <= 0) {
607 io17 = .5;
608
609 io18 = -.3 * i_object[IDOF_PELVIS_RADIUS] * object0 * object8 / checker + .2 * i_object[18];
610 io19 = -.3 * i_object[IDOF_PELVIS_RADIUS] * object1 * object8 / checker + .2 * i_object[19];
611
612 // Set the mojo...
613 // ===============
614 object18 = 800 * (io17 > 0) * (io17 - A[object][2][1]);
615 } else {
616 io18 = i_object[18];
617 io19 = i_object[19];
618 }
619 }
620 }
621 } // End of climbing nonsense...
622
623 // We might for now want to set some external forces on the pelvis...
624 // ==================================================================
pelvis_set_control(int32_t pelvis,Q forward,Q turn,Q sidestep,Q lean,Q jump,int32_t crouch)625 void pelvis_set_control(int32_t pelvis, Q forward, Q turn, Q sidestep, Q lean, Q jump, int32_t crouch) {
626 const Q pi_by_two = 1.5707; // Yea, flixpoint...
627
628 sincos(S[pelvis][3][0], &object0, &object1);
629
630 // Get rid of it all...
631 // --------------------
632 I[pelvis][15] = I[pelvis][16] = I[pelvis][17] = I[pelvis][18] = I[pelvis][19] = I[pelvis][7] = 0;
633
634 // Here's the thrust of the situation...
635 // -------------------------------------
636 I[pelvis][18] = forward * object1 * I[pelvis][IDOF_PELVIS_MASS];
637 I[pelvis][19] = forward * object0 * I[pelvis][IDOF_PELVIS_MASS];
638
639 // And the sidestep is off by pi/two...
640 // ------------------------------------
641 sincos((S[pelvis][3][0] - pi_by_two), &object0, &object1);
642 I[pelvis][18] += sidestep * object1 * I[pelvis][IDOF_PELVIS_MASS];
643 I[pelvis][19] += sidestep * object0 * I[pelvis][IDOF_PELVIS_MASS];
644
645 // And the turn of the...
646 // ----------------------
647 I[pelvis][16] = turn * I[pelvis][IDOF_PELVIS_MOI];
648
649 // Jump jets of joy...
650 // -------------------
651 if (jump > 0)
652 I[pelvis][17] = .003 * I[pelvis][IDOF_PELVIS_MASS] * jump;
653 if (jump < 0)
654 I[pelvis][17] = .0006 * I[pelvis][IDOF_PELVIS_MASS] * jump;
655
656 // And finally leaning about...
657 // ----------------------------
658 I[pelvis][15] = .04 * lean * I[pelvis][1]; // Exactly the angle!
659
660 // Crouching (overpowers jumping )...
661 // ----------------------------------
662 if (crouch > 0)
663 I[pelvis][7] = .20 * crouch * I[pelvis][1];
664
665 // Wake up...
666 // ==========
667 no_no_not_me[pelvis] =
668 (abs(I[pelvis][15]) + abs(I[pelvis][16]) + abs(I[pelvis][17]) + abs(I[pelvis][18]) + abs(I[pelvis][19]) > 0) ||
669 (no_no_not_me[pelvis] == 1);
670 }
671
672 // Sets up everything needed to manufacture a pelvis with initial state vector
673 // init_state[][] and EDMS motion parameters params[] into soliton. Returns the
674 // object number, or else a negative error code (see Soliton.CPP for error handling and codes).
675 // ============================================================================================
make_pelvis(Q init_state[6][3],Q params[10])676 int32_t make_pelvis(Q init_state[6][3], Q params[10]) {
677 // Have some variables...
678 // ======================
679 int32_t object_number = -1, // Three guesses...
680 error_code = -1; // Guilty until...
681
682 // We need ignorable coordinates...
683 // ================================
684 extern void null_function(int);
685
686 // First find out which object we're going to be...
687 // ================================================
688 while (S[++object_number][0][0] > END)
689 ; // Jon's first C trickie...
690
691 // Is it an allowed object number? Are we full? Why are we here? Is there a God?
692 // ==============================================================================
693 if (object_number < MAX_OBJ) {
694
695 // Now we can create the pelvis: first dump the initial state vector...
696 // =====================================================================
697 for (int32_t coord = 0; coord < 6; coord++) {
698 for (int32_t deriv = 0; deriv < 3; deriv++) { // Has alpha now...
699 S[object_number][coord][deriv] = A[object_number][coord][deriv] =
700 init_state[coord][deriv]; // For collisions...
701 }
702 }
703
704 // Put in the appropriate pelvis parameters...
705 // ===========================================
706 for (int copy = 0; copy < 10; copy++) {
707 I[object_number][copy + 20] = params[copy];
708 }
709
710 I[object_number][IDOF_MODEL] = PELVIS; // Hey, you are what you eat.
711
712 // We need some information that won't fit in the usual areas...
713 // =============================================================
714 // I[object_number][0] = //For reference...
715 I[object_number][6] = .5 * I[object_number][IDOF_PELVIS_RADIUS];
716 I[object_number][1] = 20 * I[object_number][IDOF_PELVIS_MOI];
717 I[object_number][2] = 4 * sqrt(I[object_number][IDOF_PELVIS_MOI] * I[object_number][1]);
718
719 // Put in the collision information...
720 // ===================================
721 I[object_number][31] = I[object_number][IDOF_PELVIS_RADIUS];
722 I[object_number][32] = I[object_number][33] = I[object_number][34] = I[object_number][35] = 0;
723 I[object_number][36] = I[object_number][IDOF_PELVIS_MASS_RECIP]; // Shrugoff "mass"...
724 I[object_number][IDOF_COLLIDE] = -1;
725 I[object_number][IDOF_AUTODESTRUCT] = 0; // No kill I...
726
727 // Zero the control initially...
728 // =============================
729 I[object_number][7] = I[object_number][8] = I[object_number][9] = I[object_number][15] = I[object_number][16] =
730 I[object_number][18] = I[object_number][19] = I[object_number][17] = 0;
731
732 // Now tell Soliton where to look for the equations of motion...
733 // =============================================================
734 idof_functions[object_number] = pelvis_idof;
735
736 equation_of_motion[object_number][0] = // Nice symmetries, huh.
737 equation_of_motion[object_number][1] = equation_of_motion[object_number][2] =
738 equation_of_motion[object_number][3] = equation_of_motion[object_number][4] =
739 equation_of_motion[object_number][5] = null_function;
740
741 // Wake me up...
742 // =============
743 no_no_not_me[object_number] = 1;
744
745 // Things seem okay...
746 // ===================
747 error_code = object_number;
748 }
749
750 // Inform the caller...
751 // ====================
752 return error_code;
753 }
754
755 // ATTENZIONE: Los parametros del model son:
756 // ==========================================
757
758 // Number | Comment
759 // --------------------
760 // 0 | K
761 // 1 | d
762 // 2 | Radius
763 // 3 | Rolling Drag
764 // 4 | 1/Mass
765 // 5 | gravity
766 // 6 | mass
767 // 7 | 1/moi
768 // 8 | rotational drag
769 // 9 | moi
770 // ==========================================
771 // So there.
772
773 // For mark...
774 // -----------
775 extern "C" {
776
EDMS_pelvis_is_climbing()777 bool EDMS_pelvis_is_climbing()
778 {
779 return pelvis_is_climbing;
780 }
781
EDMS_lean_o_meter(physics_handle ph,fix & lean,fix & crouch)782 void EDMS_lean_o_meter(physics_handle ph, fix &lean, fix &crouch) {
783
784 lean = crouch = 0;
785
786 // Are you for real?
787 // -----------------
788 if (ph > -1) {
789
790 int32_t on = ph2on[ph];
791
792 // Are you a pelvis...
793 // -------------------
794 if (I[on][IDOF_MODEL] == PELVIS) {
795 lean = S[on][5][0].to_fix();
796 crouch = I[on][0].to_fix() - 3 * V_floor[2].to_fix();
797
798 } // Pelvis check...
799
800 } // For real...
801 }
802
803 #pragma require_prototypes on
804 }
805