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 // Robot.cc is a test object for the Citadel physics system. It uses the Citadel database
20 // for B/C, and should be fairly simple and robust. Use the vector integrator!
21 // ============================================================================
22
23 // Seamus, June 29, 1993...
24 // ========================
25
26 #include <iostream>
27 //#include <conio.h>
28 #include "edms_int.h" //This is the object type library. It is universal.
29 #include "idof.h"
30 //#ifdef EDMS_SHIPPABLE
31 ////#include <mout.h>
32 //#endif
33
34 #include "edms_chk.h"
35
36 // extern "C" {
37 //#include <i86.h>
38 //#include <dpmi.h>
39 #include "ss_flet.h"
40 //}
41
42 // State information and utilities...
43 // ==================================
44 extern EDMS_Argblock_Pointer A;
45 extern Q S[MAX_OBJ][7][4], I[MAX_OBJ][DOF_MAX];
46 extern int32_t no_no_not_me[MAX_OBJ];
47
48 #define SOLITION_FRAME_CNT
49
50 // Functions...
51 // ============
52 extern void (*idof_functions[MAX_OBJ])(int32_t), (*equation_of_motion[MAX_OBJ][7])(int32_t);
53
54 // Callbacks themselves...
55 // -----------------------
56 extern void (*EDMS_object_collision)(physics_handle caller, physics_handle victim, int32_t badness, int32_t DATA1,
57 int32_t data2, fix loc[3]),
58 (*EDMS_wall_contact)(physics_handle caller);
59
60 // extern int are_you_there( int ); //Collisions...
61 // extern int check_for_hit( int );
62
63 static Q fix_one = 1., point_five = .5, two_pi = 6.283185;
64
65 // Just a thought...
66 // =================
67 static Q object0, object1, object2, object3, object4, // Howzat??
68 object5, object6, object7, object8, object9, object10, object11, object12, object13, object14, object15, object16,
69 object17, object18, object19;
70
71 // First, here are the equations of motion (outdated!)...
72 // ======================================================
73 int32_t EDMS_robot_global_badness_indicator = 0;
74
75 // Variables that are NOT on the stack...
76 // ======================================
77 static Q A00, A10, A20, A30, A01, A11, A21, A31;
78
79 static Q checker, check0, check1, check2, V_wall0, V_wall1;
80
81 static Q drug, butt;
82
83 const Q wt_pos = 0.001, wt_neg = -wt_pos;
84
85 #pragma require_prototypes off
86
87 // Here are the internal degrees of freedom:
88 // =========================================
robot_idof(int32_t object)89 void robot_idof(int32_t object) {
90
91 // Call me instead of having special code everywhere...
92 // ====================================================
93 extern void shall_we_dance(int object, Q &result0, Q &result1, Q &result2);
94
95 A00 = A[object][0][0]; // Dereference NOW!
96 A10 = A[object][1][0];
97 A20 = A[object][2][0];
98 A30 = A[object][3][0];
99 A01 = A[object][0][1];
100 A11 = A[object][1][1];
101 A21 = A[object][2][1];
102 A31 = A[object][3][1];
103
104 I[object][17] = 0; // Make sure we REALLY want to climb...
105
106 EDMS_robot_global_badness_indicator = 0;
107
108 // Get the info...
109 indoor_terrain(A00, A10, A20, I[object][IDOF_ROBOT_RADIUS], on2ph[object], TFD_FULL);
110
111 // if (EDMS_robot_global_badness_indicator != 0 ) {
112 // mout << "!Robot.cc: thinks that X: " << A00 << ", Y: " << A10 << ", Z: "
113 // << A20 << "\n"; mout << "!Robot.cc: has object number " << object << ",
114 // ph: " << on2ph[object] << "\n";
115 // }
116
117 // Boy, will this be faster in the new order...
118 // ============================================
119 Q w0, w1, w2, w3, w4;
120 w0.fix_to(terrain_info.wx);
121 w1.fix_to(terrain_info.wy);
122 w2.fix_to(terrain_info.wz);
123
124 // w3 = sqrt( w0*w0 + w1*w1 + w2*w2 );
125 // w4 = 2*( w3 - .5*I[object][22] );
126 // if ( w4 < 0 ) w4 = 0; //Ouch!
127 // w4 = w4 / w3;
128 // w0 *= w4; w1 *= w4; w2 *= w4;
129
130 if (w0 == 0)
131 check0 = 1;
132 else
133 check0 = 0;
134
135 if (w1 == 0)
136 check1 = 1;
137 else
138 check1 = 0;
139
140 if ((terrain_info.fz == 0) && (terrain_info.cz == 0))
141 check2 = 1;
142 else
143 check2 = 0;
144
145 object0.fix_to(terrain_info.fx + terrain_info.cx);
146 object1.fix_to(terrain_info.fy + terrain_info.cy);
147 object2.fix_to(terrain_info.fz + terrain_info.cz);
148
149 object0 += w0;
150 object1 += w1;
151 object2 += w2;
152
153 checker = sqrt(object0 * object0 + object1 * object1 + object2 * object2);
154
155 if (checker > .0005) {
156 object3 = fix_one / checker; // To get primitive...
157 // mout << "NZero!! " << checker << "\n";
158 } else
159 checker = object3 = 0;
160
161 object4 = object3 * object0; // The primitive V_n...
162 object5 = object3 * object1;
163 object6 = object3 * object2;
164
165 object7 = .75 * I[object][21] *
166 (A01 * object4 // Delta_magnitude...
167 + A11 * object5 + A21 * object6);
168
169 object8 = I[object][20]; // Omega_magnitude...
170 // if( I[object][10]<0 ) object7 *= 4;
171 // if( I[object][10]<0 ) object8 *= 2;
172
173 // mout << "o7: " << object7 << "\n";
174 object4 = object7 * object4; // Delta...
175 object5 = object7 * object5;
176 object6 = object7 * object6;
177
178 object9 = ((checker > .001) || (I[object][10] > 0)); // Are we in the rub???
179
180 // Let's not power through the walls anymore...
181 // --------------------------------------------
182 I[object][18] *= check0;
183 I[object][19] *= check1;
184
185 // Here are collisions with other objects...
186 // =========================================
187 object10 = object11 = object12 = 0;
188
189 if (I[object][5] == 0) {
190 shall_we_dance(object, object10, object11, object12);
191 object10 *= I[object][20] * check0; // More general than it was...
192 object11 *= I[object][20] * check1;
193 // object12 *= I[object][20]*check2;
194 }
195
196 // Climbing overriden with repulsors...
197 // ====================================
198 if (ss_edms_bcd_flags & SS_BCD_REPUL_ON) {
199
200 // Get the speed...
201 Q repulsor_speed = 21;
202 if ((ss_edms_bcd_flags & SS_BCD_REPUL_SPD) == SS_BCD_REPUL_NORM)
203 repulsor_speed = 7;
204
205 // Assume we're going up, unless...
206 if ((ss_edms_bcd_flags & SS_BCD_REPUL_TYPE) == SS_BCD_REPUL_DOWN)
207 repulsor_speed *= -.5;
208
209 // The parameter should be the desired height....
210 Q repul_height;
211 repul_height.fix_to(ss_edms_bcd_param);
212
213 Q nearness_or_something = repul_height - A[object][2][0];
214 if (abs(nearness_or_something) <= .333) {
215 repulsor_speed *= 3 * nearness_or_something;
216 }
217
218 Q io17 = repulsor_speed;
219
220 I[object][17] = I[object][26] * ((io17 - A[object][2][1]) + I[object][25]);
221
222 object9 = 1;
223 }
224
225 // AutoClimbing(tm) is for wussies (is superseeded by climbing)...
226 // ===============================================================
227 if ((ss_edms_bcd_flags & SS_BCD_MISC_STAIR)) {
228
229 Q o1 = 0, o0 = 0;
230
231 if ((checker > 0) && (abs(I[object][18]) + abs(I[object][19]) > .01)) {
232
233 Q ratio = (I[object][18] + A[object][0][1]) * object0 + (I[object][19] + A[object][1][1]) * object1;
234
235 Q io17 = .5;
236
237 if (ratio <= 0) {
238 o1 = object1;
239 o0 = object0;
240 } else
241 o1 = o0 = io17 = 0;
242
243 I[object][18] = -.3 * I[object][22] * o0 * object8 / checker + .1 * I[object][18];
244 I[object][19] = -.3 * I[object][22] * o1 * object8 / checker + .1 * I[object][19];
245 // io18 = -.3*I[object][22]*o0*object8/checker + .1*I[object][18];
246 // io19 = -.3*I[object][22]*o1*object8/checker + .1*I[object][19];
247
248 // Set the mojo...
249 // ===============
250 I[object][17] = 800 * (io17 - A[object][2][1]);
251 }
252 }
253
254 // Angular play (citadel) ...
255 // ==========================
256 if (S[object][3][0] > two_pi)
257 S[object][3][0] -= two_pi;
258 if (S[object][3][0] < -two_pi)
259 S[object][3][0] += two_pi;
260
261 // Don't be stupid...
262 // ------------------
263 drug = -object9 * I[object][23];
264 // mout << drug << "\n";
265 butt = I[object][24];
266
267 // Try the equations of motion here for grins...
268 // =============================================
269 S[object][2][2] = butt * (object8 * object2 // Elasticity...
270 - object6 // Drag...
271 + I[object][17] // Control...
272 + drug * A21 + object12)
273
274 - I[object][25]; // Grav'ty...
275
276 S[object][0][2] = butt * (object8 * object0 // Elasticity...
277 - object4 // Drag...
278 + object9 * I[object][18] // Control...
279 + drug * A01 // Drag...
280 + object10); // Collide...
281
282 S[object][1][2] = butt * (object8 * object1 // Elasticity...
283 - object5 // Drag...
284 + object9 * I[object][19] // Control...
285 + drug * A11 // Drag...
286 + object11); // Collide...
287
288 S[object][3][2] = I[object][27] * (I[object][16] // Control...
289 - I[object][28] * A31); // Drag...
290
291 // mout << "Butt: " << butt << "\n";
292 // mout << "1X: " << object0 << " 1Y: " << object1 << " 1Z: " << object2 << "\n";
293 // mout << "VX: " << A01 << " VY: " << A11 << " VZ: " << A21 << "\n";
294 // mout << "2X: " << object4 << " 2Y: " << object5 << " 2Z: " << object6 << "\n";
295 // mout << "3X: " << object8*object0 << " 3Y: " << object8*object1 << " 3Z: " << object8*object2 << "\n";
296 // mout << "FX: " << object8*object0 - object4 << " FY: " << object8*object1 - object5 << " FZ: " <<
297 // object8*object2 - object6 << "\n"; mout << "xx: " << drug*A01 << " yy: " << drug*A11 << " zz: " <<
298 // drug*A21 << "\n";
299 // mout << " ZZ: " << S[object][2][2] << " : " << butt*(object8*object2 - object6) - I[object][25] <<
300 // " : " << drug*A21 << " : " << object12 << " : " << I[object][17] << "\n";
301 // mout << I[object][17] << " : " << object << "\n";
302
303 // Damnage...
304 // ==========
305 Q dam0 = object8 * object0 - object4;
306 Q dam1 = object8 * object1 - object5;
307 Q dam2 = object8 * object2 - object6;
308
309 I[object][14] = I[object][IDOF_ROBOT_MASS_RECIP] * (abs(dam0) + abs(dam1) + abs(dam2)); // Damage??
310
311 // Is there a projectile hit?
312 // ==========================
313 if (I[object][35] > 0) {
314
315 // Let's not power through the walls anymore...
316 // --------------------------------------------
317 // mout << "knock " << I[object][32] << " " << I[object][33] << " " << I[object][34] << ": I[24] " <<
318 // I[object][24] << "\n";
319
320 // if (I[object][24] > 1.0)
321 // {
322 // if ( abs(I[object][32]) > 1000 ) I[object][32] = 1000*(1-2*(I[object][32]<0));
323 // if ( abs(I[object][33]) > 1000 ) I[object][33] = 1000*(1-2*(I[object][33]<0));
324 // if ( abs(I[object][34]) > 1000 ) I[object][34] = 1000*(1-2*(I[object][34]<0));
325 // }
326
327 // mout << "clamp " << I[object][32] << " " << I[object][33] << " " << I[object][34] << "\n";
328
329 S[object][0][2] += /* I[object][24]* */ I[object][32] * check0;
330 S[object][1][2] += /* I[object][24]* */ I[object][33] * check1;
331 S[object][2][2] += /* I[object][24]* */ I[object][34] * check2;
332
333 // mout << " add " << /* I[object][24]* */ I[object][32]*check0 << " " <<
334 // /* I[object][24]* */ I[object][33]*check0 << " " <<
335 // /* I[object][24]* */ I[object][34]*check0 << "\n";
336
337 // mout << "R: " << object << " K: " << I[object][24]*I[object][32]*check0 << " : " <<
338 // I[object][24]*I[object][33]*check1 << " : " << I[object][24]*I[object][34] << "\n";
339
340 I[object][35] = 0;
341 I[object][32] = 0;
342 I[object][33] = 0;
343 I[object][34] = 0;
344 }
345
346 // That's all, folks...
347 // ====================
348 }
349
350 // We might for now want to set some external forces on the robot...
351 // ==================================================================
robot_set_control(int32_t robot,Q thrust_lever,Q attitude_jet,Q jump)352 void robot_set_control(int32_t robot, Q thrust_lever, Q attitude_jet, Q jump) {
353
354 sincos(S[robot][3][0], &object0, &object1);
355
356 #ifdef EDMS_SHIPPABLE
357 if (I[robot][30] != ROBOT)
358 mout << "You are an idiot: I'm not a ROBOT!\n";
359 #endif
360
361 // Here's the thrust of the situation...
362 // -------------------------------------
363 I[robot][18] = thrust_lever * object1 * I[robot][IDOF_ROBOT_MASS];
364 I[robot][19] = thrust_lever * object0 * I[robot][IDOF_ROBOT_MASS];
365 I[robot][17] = I[robot][26] * jump;
366
367 // And the turn of the...
368 // ----------------------
369 I[robot][16] = attitude_jet * I[robot][IDOF_ROBOT_MOI];
370
371 // Wakee wakee...
372 // --------------
373 no_no_not_me[robot] = (abs(I[robot][18]) + abs(I[robot][19]) + abs(I[robot][16]) + abs(I[robot][17]) > 0);
374 }
375
376 // Here is a separate control routine for robots under AI domination...
377 // ====================================================================
robot_set_ai_control(int32_t robot,Q desired_heading,Q desired_speed,Q sidestep,Q urgency,Q & there_yet,Q distance)378 void robot_set_ai_control(int32_t robot, Q desired_heading, Q desired_speed, Q sidestep, Q urgency, Q &there_yet,
379 Q distance) {
380
381 const Q one_by_pi = 0.31830, pi = 3.14159, two_pi = 6.28318;
382
383 #ifdef EDMS_SHIPPABLE
384 if (I[robot][30] != ROBOT)
385 mout << "Hey, don't call control_robot on non-robots!\n";
386 #endif
387
388 if (desired_heading > two_pi)
389 desired_heading -= two_pi;
390 if (desired_heading < 0)
391 desired_heading += two_pi;
392
393 // Nota bene: Here the desired heading is specified is in the range
394 // 0 <= desired_heading < 2pi. Urgency is a number in the range
395 // 0 <= urgency <= 20. A zero urgency will produce no control input.
396 // ==================================================================
397
398 // Setup...
399 // --------
400 Q speed = sqrt(S[robot][0][1] * S[robot][0][1] + S[robot][1][1] * S[robot][1][1]),
401 direction = desired_heading - S[robot][3][0];
402
403 sincos(S[robot][3][0], &object0, &object1);
404
405 // Heading...
406 // ----------
407 if (direction > pi)
408 direction = -(direction - pi);
409 if (direction <= -pi)
410 direction = -(direction + pi);
411
412 // Inform the caller if we're on course yet...
413 // -------------------------------------------
414 there_yet = one_by_pi * direction * (1 - 2 * (direction < 0));
415
416 // Set the control...
417 // ------------------
418 I[robot][16] = .1 * urgency * direction * I[robot][29];
419
420 // Speed...
421 // --------
422 I[robot][17] = urgency * (1 / (10 * there_yet + 5)) * (desired_speed - speed); // temporary...
423 if (I[robot][17] < 0)
424 I[robot][17] = 0;
425
426 if (distance < 1)
427 I[robot][17] *= distance;
428
429 I[robot][18] = object1 * I[robot][17] + object0 * sidestep;
430 I[robot][19] = object0 * I[robot][17] - object1 * sidestep;
431 I[robot][17] = 0; // No jumping for AIs
432
433 // Wakee wakee...
434 // --------------
435 if (no_no_not_me[robot] == 0) {
436 no_no_not_me[robot] = (abs(I[robot][18]) + abs(I[robot][19]) + abs(I[robot][16]) > 0);
437 // if ( no_no_not_me[robot] != 0 ) mout << "R: " << robot << ", ph= " << on2ph[robot] << " awoken! " <<
438 // no_no_not_me[robot] << "\n"; mout << ( abs( I[robot][18] ) + abs( I[robot][19] ) + 50*abs(
439 // I[robot][16] ) + abs( I[robot][17] ) ) << "\n"; mout << no_no_not_me[robot] << "\n";
440 }
441
442 // mout << "Robot #" << robot << " with: " << ( abs( I[robot][18] ) + abs( I[robot][19] ) + 50*abs( I[robot][16] )
443 //+ abs( I[robot][17] ) ) << ".\n";;
444 }
445
make_robot(Q init_state[6][3],Q params[10])446 int32_t make_robot(Q init_state[6][3], Q params[10]) {
447
448 // Sets up everything needed to manufacture a robot with initial state vector
449 // init_state[][] and EDMS motion parameters params[] into soliton. Returns the
450 // object number, or else a negative error code (see Soliton.CPP for error handling and codes).
451 // ============================================================================================
452
453 // Have some variables...
454 // ======================
455 int32_t object_number = -1, // Three guesses...
456 error_code = -1; // Guilty until...
457
458 // We need ignorable coordinates...
459 // ================================
460 extern void null_function(int32_t);
461
462 // First find out which object we're going to be...
463 // ================================================
464 while (S[++object_number][0][0] > END)
465 ; // Jon's first C trickie...
466
467 // Is it an allowed object number? Are we full? Why are we here? Is there a God?
468 // ==============================================================================
469 if (object_number < MAX_OBJ) {
470
471 // Now we can create the robot: first dump the initial state vector...
472 // =====================================================================
473 for (int32_t coord = 0; coord < 6; coord++) {
474 for (int32_t deriv = 0; deriv < 3; deriv++) { // Has alpha now...
475 S[object_number][coord][deriv] = A[object_number][coord][deriv] =
476 init_state[coord][deriv]; // For collisions...
477 }
478 }
479
480 // Put in the appropriate robot parameters...
481 // ===========================================
482 for (int32_t copy = 0; copy < 10; copy++) {
483 I[object_number][copy + 20] = params[copy];
484 }
485 I[object_number][IDOF_MODEL] = ROBOT; // Hey, you are what you eat.
486
487 // Put in the collision information...
488 // ===================================
489 I[object_number][IDOF_RADIUS] = I[object_number][IDOF_ROBOT_RADIUS];
490 I[object_number][32] = I[object_number][33] = I[object_number][34] = I[object_number][35] = 0;
491 I[object_number][36] = I[object_number][IDOF_ROBOT_MASS_RECIP]; // Shrugoff "mass"...
492 I[object_number][IDOF_COLLIDE] = -1;
493 I[object_number][IDOF_AUTODESTRUCT] = 0; // No kill I...
494
495 // Turn ON collisions for this robot...
496 // ------------------------------------
497 I[object_number][5] = 0; // negative values are off...
498
499 // Zero the control initially...
500 // =============================
501 I[object_number][16] = I[object_number][18] = I[object_number][19] = I[object_number][17] = 0;
502
503 // Now tell Soliton where to look for the equations of motion...
504 // =============================================================
505 idof_functions[object_number] = robot_idof;
506
507 equation_of_motion[object_number][0] = equation_of_motion[object_number][1] =
508 equation_of_motion[object_number][2] = equation_of_motion[object_number][3] =
509 equation_of_motion[object_number][4] = // Nice symmetries, huh.
510 equation_of_motion[object_number][5] = null_function;
511
512 // for (int tt = 0; tt < 10; tt++ ) mout << params[tt] << " : ";
513 // mout << "\n";
514
515 // Wakee wakee...
516 // --------------
517 no_no_not_me[object_number] = 1;
518
519 // Things seem okay...
520 // ===================
521 error_code = object_number;
522 }
523
524 // Inform the caller...
525 // ====================
526 return error_code;
527 }
528
529 #pragma require_prototypes on
530
531 // ATTENZIONE: Los parametros del model son:
532 // ==========================================
533
534 // Number | Comment
535 // --------------------
536 // 0 | K
537 // 1 | d
538 // 2 | Radius
539 // 3 | Rolling Drag
540 // 4 | 1/Mass
541 // 5 | gravity
542 // 6 | mass
543 // 7 | 1/moi
544 // 8 | rotational drag
545 // 9 | moi
546 // ==========================================
547 // So there.
548