1 /****************************************************************************
2 * mechsim.cpp
3 *
4 * Author: Christoph Hormann <chris_hormann@gmx.de>
5 *
6 * Mechanics simulation
7 *
8 * Copyright 2002-2005 Christoph Hormann
9 *
10 * from Persistence of Vision(tm) Ray Tracer version 3.6.
11 * Copyright 1991-2003 Persistence of Vision Team
12 * Copyright 2003-2004 Persistence of Vision Raytracer Pty. Ltd.
13 *---------------------------------------------------------------------------
14 * NOTICE: This source code file is provided so that users may experiment
15 * with enhancements to POV-Ray and to port the software to platforms other
16 * than those supported by the POV-Ray developers. There are strict rules
17 * regarding how you are permitted to use this file. These rules are contained
18 * in the distribution and derivative versions licenses which should have been
19 * provided with this file.
20 *
21 * These licences may be found online, linked from the end-user license
22 * agreement that is located at http://www.povray.org/povlegal.html
23 *---------------------------------------------------------------------------
24 * This program is based on the popular DKB raytracer version 2.12.
25 * DKBTrace was originally written by David K. Buck.
26 * DKBTrace Ver 2.0-2.12 were written by David K. Buck & Aaron A. Collins.
27 *---------------------------------------------------------------------------
28 *
29 *===========================================================================
30 * This file is part of MegaPOV, a modified and unofficial version of POV-Ray
31 * For more information on MegaPOV visit our website:
32 * http://megapov.inetart.net/
33 *===========================================================================
34 *
35 * $RCSfile: mechsim.cpp,v $
36 * $Revision: 1.27 $
37 * $Author: chris $
38 *
39 *****************************************************************************/
40
41 /*
42
43 The MechSim patch
44 =================
45
46 History:
47
48 SimPOV 0.1.0: October 2002
49 -----------------------------------
50
51 - the basics: masses, connections, faces, environments
52
53
54 SimPOV 0.2.0: December 2002
55 -----------------------------------
56
57 - bugfixes
58
59
60 MegaPOV 1.0: December 31, 2002
61 -----------------------------------
62
63 - adding attachments, fields, interactions
64 - other improvements
65
66
67 MegaPOV 1.1: December 31, 2002
68 -----------------------------------
69
70 - Connection-connection collisions
71 - collision bounding/hashing system by Daniel Jungmann
72
73
74 MegaPOV 1.2:
75 -----------------------------------
76
77 - improved collision bounding/hashing system by Daniel Jungmann
78 - viscoelastic connections
79 - global fixation in 1 or 2 dimensions
80 - attachments can be limited to 1 or two dimensions
81 (free movement in the others)
82 - adaptive time stepping for 4th order Runge-Kutta method
83 - new simulation data file format
84 - custom forces that can be applied to individual masses
85 - more detailed simulation statistics
86 - access to simulation forces from SDL
87 - various bug fixes and improvements
88
89
90 */
91
92 #include "frame.h"
93 #include "povray.h"
94 #include "vector.h"
95 #include "function.h"
96 #include "fnpovfpu.h"
97 #include "pov_util.h"
98 #include "mesh.h"
99 #include "ray.h"
100 #include "objects.h"
101 #include "povmsend.h"
102 #include "mechsim.h"
103
104 #include <algorithm>
105
106 #ifdef MECHANICS_SIMULATION_PATCH /* Christoph Hormann August 2002 */
107
108 BEGIN_POV_NAMESPACE
109
110 /*****************************************************************************
111 * Local preprocessor defines
112 ******************************************************************************/
113
114 //#define MECHSIM_DEBUG 1
115
116 #define MAX_STR 1024
117
118 #define MECHSIM_GRADIENT_ACCURACY 0.001
119
120 #define MECHSIM_EPSILON 0.0001
121
122 #define MECHSIM_BOUNDING_HASH_THRESHOLD 100
123 #define MECHSIM_BOUNDING_BOX_THRESHOLD 10
124
125 // bounding
126
127 #define INT unsigned int
128
129 #define testbit(ss, ibit) (((ss)[BIT2INT(ibit)] & BIT2MASK(ibit)) != 0)
130 #define setbit(ss, ibit) ((ss)[BIT2INT(ibit)] |= BIT2MASK(ibit))
131
132 #define BITSPERINT (8*sizeof(INT))
133 #define NINTS(nbits) (((nbits) + BITSPERINT - 1) / BITSPERINT)
134
135 #define BIT2INT(ibit) ((ibit) / BITSPERINT)
136 #define BIT2SHIFT(ibit) ((ibit) % BITSPERINT)
137 #define BIT2MASK(ibit) (1 << BIT2SHIFT(ibit))
138
139 // adaptive time stepping
140
141 const DBL Mechsim_Safety_Fact = 0.9;
142 const DBL Mechsim_Exp_Inc = 0.2;
143 const DBL Mechsim_Exp_Dec = 0.25;
144 const DBL Mechsim_Error_Threshold = pow(5.0/Mechsim_Safety_Fact, -1.0/Mechsim_Exp_Inc);
145
146
147 /*****************************************************************************
148 * Local functions
149 ******************************************************************************/
150
151 void Function_Gradient(FUNCTION funct, VECTOR Pos, VECTOR grad);
152 int func_triangle(VECTOR Point, VECTOR CPoint, VECTOR Grad, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Ratio);
153 void Triangle_Distribute(VECTOR Point, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Weight1, DBL *Weight2, DBL *Weight3);
154 void Calc_Friction(VECTOR FFrict, VECTOR Velocity, DBL Friction, VECTOR Normal, DBL Force);
155
156 void Mechsim_Impact_Collide_Environment(int Index, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env);
157 void Mechsim_Gradient_Collide_Environment(int Index, VECTOR Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env);
158 void Mechsim_Force_Collide_Environment(int Index, VECTOR Accel,VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env);
159
160 void Mechsim_Collide_Dynamic(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
161 void Mechsim_Collide_Mass_Mass(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2);
162 void Mechsim_Collide_Mass_Face(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2);
163 void Mechsim_Collide_Connection_Connection(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2);
164
165 void Mechsim_Gradient_Connect(VECTOR *Gradient, VECTOR *Pos_Array);
166 void Mechsim_Force_Connect(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array);
167
168 void Mechsim_Gradient_Viscoelastics(VECTOR *Gradient, VECTOR *Pos_Array);
169 void Mechsim_Force_Viscoelastics(VECTOR *Accel, VECTOR *State_Array);
170 void Mechsim_Integrate_Viscoelastics(VECTOR *State_Array);
171
172 void Mechsim_Gradient_Interactions(VECTOR *Accel, VECTOR *Pos_Array);
173 void Mechsim_Force_Interactions(VECTOR *Accel, VECTOR *Pos_Array);
174
175 void Mechsim_Force_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array);
176 void Mechsim_Gradient_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array);
177
178 void Mechsim_Force_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array);
179 void Mechsim_Gradient_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array);
180
181 void Mechsim_Gradients(VECTOR *Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data);
182 void Mechsim_Forces(VECTOR *Forces, VECTOR *State_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data);
183 void Mechsim_Check_Col(VECTOR Pos, VECTOR Old_Pos);
184 void Mechsim_Check_Instability(VECTOR Pos, VECTOR Vel, VECTOR Accel);
185
186 void Mechsim_Integrate_Euler(VECTOR *State_Array, VECTOR *Forces);
187 void Mechsim_Integrate_Heun(VECTOR *State_Array, VECTOR *Forces);
188 void Mechsim_Integrate_Runge_Kutta4(VECTOR *State_Array, VECTOR *Forces);
189 void Mechsim_Integrate_Gradient(VECTOR *State_Array, VECTOR *Forces);
190
191 void Mechsim_Create_Groups(int Count);
192 void Mechsim_Prepare_Data(void);
193 bool Mechsim_save_file (void);
194
195 char *Read_Vector(IStream *fd, char *str, char *buffer, VECTOR Vect, bool *got_eof);
196 char *Read_Float(IStream *fd, char *str, char *buffer, double *Value, bool *got_eof);
197 char *Read_Int(IStream *fd, char *str, char *buffer, int *Value, bool *got_eof);
198
199 static void calc_faces_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
200 static bool intersect_sphere_box(BBOX BBox, VECTOR Center, DBL Radius);
201 static void calc_faces_bounding_hashtable(MECHSIM_BOUNDING_DATA* Bounding_Data);
202 static void insert_index_bounding_hastable(BOUNDING_HASHTABLE_ITEM* bounding_hashtable, unsigned int hash, int index, int time_stamp);
203 static void intersect_sphere_bounding_hashtable(VECTOR Center, DBL Radius, MECHSIM_BOUNDING_DATA* Bounding_Data);
204 static void calc_face_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
205 static unsigned int bounding_hash_func(long int x, long int y, long int z, long int n);
206 static bool intersect_box_box(BBOX BBox_1, BBOX BBox_2);
207 static void calc_face_group_intersection(BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data);
208 static MECHSIM_BOUNDING_DATA* init_bounding(void);
209 static void free_bounding(MECHSIM_BOUNDING_DATA* Bounding_Data);
210 static SNGL calc_mass_unit_size(void);
211 static void calc_masses_bounding_hashtable(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
212 static void calc_mass_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
213 static void calc_mass_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data);
214 static void calc_connect_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
215 static void calc_connect_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data);
216
217 /* ------------------------------------------------------ */
218 /* global variables */
219 /* ------------------------------------------------------ */
220
221 MECHSIM_OPTIONS MechsimOptions;
222
223 /* ------------------------------------------------------ */
224 /* external variables */
225 /* ------------------------------------------------------ */
226
227 /* ------------------------------------------------------ */
228 /* static functions */
229 /* ------------------------------------------------------ */
230
231 /* ------------------------------------------------------ */
232 /* static variables */
233 /* ------------------------------------------------------ */
234
235 /*****************************************************************************
236 *
237 * FUNCTION
238 *
239 * Initialize_MechSim
240 *
241 * INPUT
242 *
243 * OUTPUT
244 *
245 * RETURNS
246 *
247 * AUTHOR
248 *
249 * Christoph Hormann
250 *
251 * DESCRIPTION
252 *
253 * Set default values of simulation data structure
254 *
255 * CHANGES
256 *
257 * --- Aug 2002 : Creation
258 *
259 ******************************************************************************/
Initialize_MechSim(void)260 void Initialize_MechSim(void)
261 {
262 /* Initialize 'MechsimOptions' fields */
263
264 MechsimOptions.Method = 1;
265 MechsimOptions.Step_Count = 10;
266 MechsimOptions.Time_Step = 0.1;
267 MechsimOptions.Time_Step_Min = 1.0e-7;
268 MechsimOptions.Accuracy = 1.0e-5;
269 MechsimOptions.Start_Time = 0.0;
270 MechsimOptions.End_Time = 0.0;
271 MechsimOptions.Time = 0.0;
272 MechsimOptions.TS_Min = 0.1;
273 MechsimOptions.TS_Max = 0.1;
274 MechsimOptions.Timing_Given = false;
275 MechsimOptions.Timing_Loaded = false;
276 Make_Vector(MechsimOptions.gravity, 0.0, 0.0, 0.0);
277 MechsimOptions.Damping = 0.0;
278 MechsimOptions.Environment = NULL;
279 MechsimOptions.Env_max_count = 0;
280 MechsimOptions.Env_count = 0;
281 MechsimOptions.Collision_Stiffness = 0.0;
282 MechsimOptions.Collision_Damping = 0.0;
283 MechsimOptions.Collision_Friction = 0.0;
284 MechsimOptions.Collision_Friction_Excess = 1.0;
285 MechsimOptions.Calc_Collision = MECHSIM_COLLISION_NONE;
286 MechsimOptions.Calc_Collision_Faces = MECHSIM_COLLISION_NONE;
287 MechsimOptions.Calc_Collision_Connections = MECHSIM_COLLISION_NONE;
288 MechsimOptions.Load_File_Name = NULL;
289 MechsimOptions.Save_File_Name = NULL;
290 MechsimOptions.Save_File = false;
291 MechsimOptions.Save_File_Type = 4;
292 MechsimOptions.Data.mass_count = 0;
293 MechsimOptions.Data.connect_count = 0;
294 MechsimOptions.Data.faces_count = 0;
295 MechsimOptions.Data.ve_count = 0;
296 MechsimOptions.Data.group_count = 0;
297
298 MechsimOptions.Data.max_masses = 0;
299 MechsimOptions.Data.max_connects = 0;
300 MechsimOptions.Data.max_faces = 0;
301 MechsimOptions.Data.max_groups = 0;
302 MechsimOptions.Data.max_viscoelastics = 0;
303
304 MechsimOptions.Enabled = false;
305
306 MechsimOptions.Interactions.Function = NULL;
307 MechsimOptions.Interactions.count = 0;
308 MechsimOptions.Interactions.max_count = 0;
309
310 MechsimOptions.Attachments.Function = NULL;
311 MechsimOptions.Attachments.Fixed = NULL;
312 MechsimOptions.Attachments.count = 0;
313 MechsimOptions.Attachments.max_count = 0;
314
315 MechsimOptions.Fields.Function = NULL;
316 MechsimOptions.Fields.count = 0;
317 MechsimOptions.Fields.max_count = 0;
318
319 MechsimOptions.Forces.Function = NULL;
320 MechsimOptions.Forces.count = 0;
321 MechsimOptions.Forces.max_count = 0;
322
323 MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_NO;
324 }
325
326
327 /*****************************************************************************
328 *
329 * FUNCTION
330 *
331 * Deinitialize_MechSim
332 *
333 * INPUT
334 *
335 * OUTPUT
336 *
337 * RETURNS
338 *
339 * AUTHOR
340 *
341 * Christoph Hormann
342 *
343 * DESCRIPTION
344 *
345 * Free all simulation data
346 *
347 * CHANGES
348 *
349 * --- Aug 2002 : Creation
350
351 ******************************************************************************/
Deinitialize_MechSim(void)352 void Deinitialize_MechSim(void)
353 {
354 int i;
355
356 if (MechsimOptions.Enabled)
357 {
358 if (MechsimOptions.Data.ve_count>0)
359 {
360 for(i=0; i<MechsimOptions.Data.ve_count; i++)
361 {
362 POV_FREE(MechsimOptions.Data.Viscoelastics[i].Element);
363 }
364 }
365
366 MechsimOptions.Data.mass_count = 0;
367 MechsimOptions.Data.connect_count = 0;
368 MechsimOptions.Data.faces_count = 0;
369 MechsimOptions.Data.ve_count = 0;
370 MechsimOptions.Data.group_count = 0;
371
372 if (MechsimOptions.Data.max_masses>0) POV_FREE(MechsimOptions.Data.Masses);
373 if (MechsimOptions.Data.max_connects>0) POV_FREE(MechsimOptions.Data.Connects);
374 if (MechsimOptions.Data.max_faces>0) POV_FREE(MechsimOptions.Data.Faces);
375 if (MechsimOptions.Data.max_viscoelastics>0) POV_FREE(MechsimOptions.Data.Viscoelastics);
376
377 if (MechsimOptions.Data.max_groups>0)
378 {
379 for(i=0; i<MechsimOptions.Data.max_groups; i++)
380 {
381 POV_FREE(MechsimOptions.Data.Groups[i].Masses);
382 POV_FREE(MechsimOptions.Data.Groups[i].Connects);
383 POV_FREE(MechsimOptions.Data.Groups[i].Faces);
384 }
385
386 POV_FREE(MechsimOptions.Data.Groups);
387 }
388
389 MechsimOptions.Data.max_masses = 0;
390 MechsimOptions.Data.max_connects = 0;
391 MechsimOptions.Data.max_faces = 0;
392 MechsimOptions.Data.max_viscoelastics = 0;
393 MechsimOptions.Data.max_groups = 0;
394
395 if (MechsimOptions.Interactions.max_count>0)
396 {
397 for (i=0; i<MechsimOptions.Interactions.count; i++)
398 Destroy_Function(MechsimOptions.Interactions.Function[i]);
399 POV_FREE(MechsimOptions.Interactions.Function);
400 }
401 if (MechsimOptions.Attachments.max_count>0)
402 {
403 for (i=0; i<MechsimOptions.Attachments.count; i++)
404 Destroy_Function(MechsimOptions.Attachments.Function[i]);
405 POV_FREE(MechsimOptions.Attachments.Function);
406 POV_FREE(MechsimOptions.Attachments.Fixed);
407 }
408 if (MechsimOptions.Fields.max_count>0)
409 {
410 for (i=0; i<MechsimOptions.Fields.count; i++)
411 Destroy_Function(MechsimOptions.Fields.Function[i]);
412 POV_FREE(MechsimOptions.Fields.Function);
413 }
414 if (MechsimOptions.Forces.max_count>0)
415 {
416 for (i=0; i<MechsimOptions.Forces.count; i++)
417 Destroy_Function(MechsimOptions.Forces.Function[i]);
418 POV_FREE(MechsimOptions.Forces.Function);
419 }
420
421 MechsimOptions.Attachments.max_count = 0;
422 MechsimOptions.Attachments.count = 0;
423 MechsimOptions.Interactions.max_count = 0;
424 MechsimOptions.Interactions.count = 0;
425 MechsimOptions.Fields.max_count = 0;
426 MechsimOptions.Fields.count = 0;
427 MechsimOptions.Forces.max_count = 0;
428 MechsimOptions.Forces.count = 0;
429
430 MechsimOptions.Enabled = false;
431 }
432
433 /* Environments */
434 if (MechsimOptions.Env_max_count>0)
435 {
436 for (i=0; i<MechsimOptions.Env_count; i++)
437 {
438 if (MechsimOptions.Environment[i].Function != NULL) Destroy_Function(MechsimOptions.Environment[i].Function);
439 if (MechsimOptions.Environment[i].Object != NULL) Destroy_Object(MechsimOptions.Environment[i].Object);
440 MechsimOptions.Environment->Function = NULL;
441 MechsimOptions.Environment->Object = NULL;
442 }
443 POV_FREE(MechsimOptions.Environment);
444 MechsimOptions.Environment = NULL;
445 MechsimOptions.Env_count = 0;
446 MechsimOptions.Env_max_count = 0;
447 }
448
449 }
450
451 /*****************************************************************************
452 *
453 * FUNCTION
454 *
455 * Function_Gradient
456 *
457 * INPUT
458 *
459 * funct - function to calculate gradient from
460 * Pos - evaluation point
461 *
462 * OUTPUT
463 *
464 * the gradient is returned via 'grad'
465 *
466 * RETURNS
467 *
468 * AUTHOR
469 *
470 * Christoph Hormann
471 *
472 * DESCRIPTION
473 *
474 * Calculates the gradient of a function at a specified point.
475 *
476 * Works just like the corresponding macro in 'math.inc'
477 *
478 * CHANGES
479 *
480 * --- Aug 2002 : Creation
481 *
482 ******************************************************************************/
483
Function_Gradient(FUNCTION funct,VECTOR Pos,VECTOR grad)484 void Function_Gradient(FUNCTION funct, VECTOR Pos, VECTOR grad)
485 {
486
487 POVFPU_SetLocal(X, Pos[X] + MECHSIM_GRADIENT_ACCURACY);
488 POVFPU_SetLocal(Y, Pos[Y]);
489 POVFPU_SetLocal(Z, Pos[Z]);
490 grad[X] = POVFPU_Run(funct);
491 POVFPU_SetLocal(X, Pos[X] - MECHSIM_GRADIENT_ACCURACY);
492 POVFPU_SetLocal(Y, Pos[Y]);
493 POVFPU_SetLocal(Z, Pos[Z]);
494 grad[X] -= POVFPU_Run(funct);
495 grad[X] /= 2.0*MECHSIM_GRADIENT_ACCURACY;
496
497 POVFPU_SetLocal(X, Pos[X]);
498 POVFPU_SetLocal(Y, Pos[Y] + MECHSIM_GRADIENT_ACCURACY);
499 POVFPU_SetLocal(Z, Pos[Z]);
500 grad[Y] = POVFPU_Run(funct);
501 POVFPU_SetLocal(X, Pos[X]);
502 POVFPU_SetLocal(Y, Pos[Y] - MECHSIM_GRADIENT_ACCURACY);
503 POVFPU_SetLocal(Z, Pos[Z]);
504 grad[Y] -= POVFPU_Run(funct);
505 grad[Y] /= 2.0*MECHSIM_GRADIENT_ACCURACY;
506
507 POVFPU_SetLocal(X, Pos[X]);
508 POVFPU_SetLocal(Y, Pos[Y]);
509 POVFPU_SetLocal(Z, Pos[Z] + MECHSIM_GRADIENT_ACCURACY);
510 grad[Z] = POVFPU_Run(funct);
511 POVFPU_SetLocal(X, Pos[X]);
512 POVFPU_SetLocal(Y, Pos[Y]);
513 POVFPU_SetLocal(Z, Pos[Z] - MECHSIM_GRADIENT_ACCURACY);
514 grad[Z] -= POVFPU_Run(funct);
515 grad[Z] /= 2.0*MECHSIM_GRADIENT_ACCURACY;
516
517 }
518
519
520 /*****************************************************************************
521 *
522 * FUNCTION
523 *
524 * func_triangle
525 *
526 * INPUT
527 *
528 * Point - point to evaluate the triangle function at
529 * P1, P2, P3 - vertices of the triangle
530 *
531 * OUTPUT
532 *
533 * the distance of Point to the specified triangle
534 *
535 * RETURNS
536 *
537 * AUTHOR
538 *
539 * Christoph Hormann
540 *
541 * DESCRIPTION
542 *
543 * Calculates the distance of a point to a triangle.
544 *
545 * Derieved from the internal function 'f_triangle' developed by ABX
546 *
547 * CHANGES
548 *
549 * --- Sep 2002 : Creation
550 *
551 ******************************************************************************/
552
func_triangle(VECTOR Point,VECTOR CPoint,VECTOR Grad,VECTOR P1,VECTOR P2,VECTOR P3,DBL * Ratio)553 int func_triangle(VECTOR Point, VECTOR CPoint, VECTOR Grad, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Ratio)
554 {
555 VECTOR v_temp,N,R1,R2,R3,E;
556 DBL Nd,Ed,El, Dist;
557 //VECTOR Distance;
558 int Result;
559
560 VSub(R1,P2,P1);
561 VSub(R2,P3,P2);
562 VSub(R3,P1,P3);
563 VNormalizeCrossNotZeroLength(N,R2,R3);
564 VNormalizeCrossNotZeroLength(E,N,R1);
565 VDot(Ed,E,P1);
566 VDot(El,E,Point);
567 if ( Ed < El )
568 {
569 VNormalizeCrossNotZeroLength(E,N,R2);
570 VDot(Ed,E,P2);
571 VDot(El,E,Point);
572 if ( Ed < El )
573 {
574 VNormalizeCrossNotZeroLength(E,N,R3);
575 VDot(Ed,E,P3);
576 VDot(El,E,Point);
577 if ( Ed < El )
578 {
579 VDot(Nd,N,P1);
580 VDot(Dist,N,Point);
581 //Distance = fabs( Distance-Nd );
582 VScale(Grad, N, Dist-Nd);
583 VAddScaled(CPoint, Point, Nd-Dist, N);
584 Result = 0;
585 }
586 else
587 {
588 //DistanceToEdge( Distance , R3 , P3 , Point );
589 VToEdge( Grad , R3 , P3 , Point );
590 VSub(v_temp, Point, P3);
591 VNormalizeEq(R3);
592 VDot(*Ratio,v_temp,R3);
593 Result = 3;
594 }
595 }
596 else
597 {
598 //DistanceToEdge( Distance , R2 , P2 , Point );
599 VToEdge( Grad , R2 , P2 , Point );
600 VSub(v_temp, Point, P2);
601 VNormalizeEq(R2);
602 VDot(*Ratio,v_temp,R2);
603 Result = 2;
604 }
605 }
606 else
607 {
608 //DistanceToEdge( Distance , R1 , P1 , Point );
609 VToEdge( Grad , R1 , P1 , Point );
610 VSub(v_temp, Point, P1);
611 VNormalizeEq(R1);
612 VDot(*Ratio,v_temp,R1);
613 Result = 1;
614 }
615
616 return Result;
617 }
618
619 /*****************************************************************************
620 *
621 * FUNCTION
622 *
623 * Triangle_Distribute
624 *
625 * INPUT
626 *
627 * Point - impact point
628 * P1, P2, P3 - vertices of the triangle
629 *
630 * OUTPUT
631 *
632 * Weight1, Weight2, Weight3 - factors for weighting
633 *
634 * RETURNS
635 *
636 * AUTHOR
637 *
638 * Christoph Hormann
639 *
640 * DESCRIPTION
641 *
642 * Weight the force on the triangle vertices according to static equilibrium
643 *
644 * CHANGES
645 *
646 * --- Sep 2002 : Creation
647 * Dec 2002 : Used a simpler method and probably fixed some problems
648 *
649 ******************************************************************************/
Triangle_Distribute(VECTOR Point,VECTOR P1,VECTOR P2,VECTOR P3,DBL * Weight1,DBL * Weight2,DBL * Weight3)650 void Triangle_Distribute(VECTOR Point, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Weight1, DBL *Weight2, DBL *Weight3)
651 {
652 DBL DSqSum1, DSqSum2, DSqSum3;
653 VECTOR Dir1, Dir2, Dir3;
654 DBL Val1, Val2, Val3;
655 DBL Scale;
656
657 /*
658 * The method used here calculates the distance of the collision point
659 * to the opposite side of the triangle. With normalized scaling (i.e.
660 * distances between 0 and 1 the sum of all distances has to be 1.0.
661 * We just have to scale the distances with their sum and get correct
662 * weights for the force.
663 */
664
665 VSub(Dir1, P2, P1);
666 DSqSum1 = VSumSqr(Dir1);
667
668 VSub(Dir2, P3, P2);
669 DSqSum2 = VSumSqr(Dir2);
670
671 VSub(Dir3, P3, P1);
672 DSqSum3 = VSumSqr(Dir3);
673
674 /* calculate the distance of the collision point to each side */
675 Val1=(Point[X] - P1[X])*Dir1[Y] - (Point[Y] - P1[Y])*Dir1[X];
676 Val2=(Point[Y] - P1[Y])*Dir1[Z] - (Point[Z] - P1[Z])*Dir1[Y];
677 Val3=(Point[Z] - P1[Z])*Dir1[X] - (Point[X] - P1[X])*Dir1[Z];
678
679 (*Weight1) = sqrt((Val1*Val1 + Val2*Val2 + Val3*Val3)/DSqSum1);
680
681 Val1=(Point[X] - P2[X])*Dir2[Y] - (Point[Y] - P2[Y])*Dir2[X];
682 Val2=(Point[Y] - P2[Y])*Dir2[Z] - (Point[Z] - P2[Z])*Dir2[Y];
683 Val3=(Point[Z] - P2[Z])*Dir2[X] - (Point[X] - P2[X])*Dir2[Z];
684
685 (*Weight2) = sqrt((Val1*Val1 + Val2*Val2 + Val3*Val3)/DSqSum2);
686
687 Val1=(Point[X] - P1[X])*Dir3[Y] - (Point[Y] - P1[Y])*Dir3[X];
688 Val2=(Point[Y] - P1[Y])*Dir3[Z] - (Point[Z] - P1[Z])*Dir3[Y];
689 Val3=(Point[Z] - P1[Z])*Dir3[X] - (Point[X] - P1[X])*Dir3[Z];
690
691 (*Weight3) = sqrt((Val1*Val1 + Val2*Val2 + Val3*Val3)/DSqSum3);
692
693 /* calculate the sum of weights */
694 Scale = 1.0/((*Weight1) + (*Weight2) + (*Weight3));
695
696 /* scale the weights */
697 (*Weight1) *= Scale;
698 (*Weight2) *= Scale;
699 (*Weight3) *= Scale;
700
701 //Status_Info("\n%g Weight=<%g, %g, %g>\n", (*Weight1) + (*Weight2) + (*Weight3), (*Weight1), (*Weight2), (*Weight3) );
702
703 }
704
705
706 /*****************************************************************************
707 *
708 * FUNCTION
709 *
710 * Calc_Friction
711 *
712 * INPUT
713 *
714 * Velocity - relative velocity to calculate the friction for
715 * Friction - friction parameters
716 * Normal - Normal vector of the friction plane
717 * Force - amount of normal (elastic) force
718 *
719 * OUTPUT
720 *
721 * FFrict - Friction force (VECTOR)
722 *
723 * RETURNS
724 *
725 * AUTHOR
726 *
727 * Christoph Hormann
728 *
729 * DESCRIPTION
730 *
731 * Calculate the friction force for various collision events
732 *
733 * CHANGES
734 *
735 * --- Jan 2003 : Creation
736 *
737 ******************************************************************************/
Calc_Friction(VECTOR FFrict,VECTOR Velocity,DBL Friction,VECTOR Normal,DBL Force)738 void Calc_Friction(VECTOR FFrict, VECTOR Velocity, DBL Friction, VECTOR Normal, DBL Force)
739 {
740 //if (Friction > 0.0)
741 //{
742
743 DBL Len;
744 VECTOR pot_force, FDir, Dir2;
745
746 // friction already slightly above the surface
747 VScale(pot_force, Normal, max(0.0, Force));
748
749 VLength(Len, Velocity);
750 if (Len>MECHSIM_EPSILON)
751 {
752 VCross(Dir2, Normal, Velocity);
753 VCross(FDir, Dir2, Normal);
754 VLength(Len, FDir);
755 if (Len>MECHSIM_EPSILON) VInverseScaleEq(FDir, Len);
756
757 VLength(Len, pot_force);
758 VScale(FFrict, FDir, Friction*Len);
759 }
760 else Make_Vector(FFrict, 0.0, 0.0, 0.0);
761
762 //}
763 //else Make_Vector(FFrict, 0.0, 0.0, 0.0);
764 }
765
766 /*****************************************************************************
767 *
768 * FUNCTION
769 *
770 * Mechsim_Impact_Collide_Environment
771 *
772 * INPUT
773 *
774 * Index - index of the mass
775 * Pos_Array, Vel_Array - state of the masses
776 * Time - current time index for the environment function
777 * Env - Pointer to Environment
778 *
779 * OUTPUT
780 *
781 * RETURNS
782 *
783 * AUTHOR
784 *
785 * Christoph Hormann
786 *
787 * DESCRIPTION
788 *
789 * Calculates collision with the environment based on impact laws.
790 *
791 * * If an environment function is given, it is evaluated and if
792 * collision is found the gradient is calculated and the velocity
793 * is mirrored at the surface and the position is updated.
794 * * If only an object is given a ray is shot from the current position
795 * in movement direction to determine the collision.
796 *
797 * Friction and damping are applied as diminishing factors (see also
798 * Christophe Bouffartique's cloth simulation)
799 *
800 * ========== WARNING ====================================================
801 *
802 * This does not necessarily work well with all integration methods,
803 * The state of the system (i.e. positions and velocities) is modified
804 * in this function instead of applying accelerations like we should do.
805 *
806 * The object based method is also problematic with higher order
807 * integration methods. Apart from that it won't successfully find the
808 * intersection in all situations like the function based method.
809 *
810 * =======================================================================
811 *
812 * CHANGES
813 *
814 * --- Sep 2002 : Creation
815 * Oct 2002 : added object intersection based method
816 *
817 ******************************************************************************/
818
Mechsim_Impact_Collide_Environment(int Index,VECTOR * Pos_Array,VECTOR * Vel_Array,DBL Time,MECHSIM_ENVIRONMENT * Env)819 void Mechsim_Impact_Collide_Environment(int Index, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env)
820 {
821 DBL Norm_Len, Fn_Val;
822 VECTOR Grad;
823
824 if (Env->Function != NULL) // ======== if function is given use it to calculate gradient ===========
825 {
826 FunctionCode *f = POVFPU_GetFunction(*Env->Function);
827
828 // ---- collision test ----
829 POVFPU_SetLocal(X, Pos_Array[Index][X]);
830 POVFPU_SetLocal(Y, Pos_Array[Index][Y]);
831 POVFPU_SetLocal(Z, Pos_Array[Index][Z]);
832 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, Time); // additional time parameter
833
834 Fn_Val = POVFPU_Run(*Env->Function);
835
836 if (Fn_Val < MechsimOptions.Data.Masses[Index].Radius)
837 {
838 Function_Gradient( *Env->Function, Pos_Array[Index], Grad);
839
840 VNormalizeEq(Grad);
841
842 /* now we do the bad thing and modify the velocity */
843 VDot(Norm_Len, Vel_Array[Index], Grad);
844 Norm_Len = -Norm_Len;
845 VAddScaledEq(Vel_Array[Index], Norm_Len, Grad); // make movement perpendicular to surface zero
846 VScaleEq(Vel_Array[Index], 1.0-Env->Friction); // apply friction
847 VAddScaledEq(Vel_Array[Index], Norm_Len*Env->Damping, Grad);
848
849 Increase_Counter(stats[MechSim_Environment_Collisions]);
850 }
851 }
852 else if (Env->Object != NULL) // ======== if only object is given use intersection test ===========
853 {
854 INTERSECTION Intersect;
855 RAY Ray;
856 VECTOR Dir, Norm;
857 DBL speed, dist;
858
859 Initialize_Ray_Containers(&Ray);
860
861 VNormalize(Dir, Vel_Array[Index]);
862 Assign_Vector(Ray.Direction, Dir);
863 Assign_Vector(Ray.Initial, Pos_Array[Index]);
864
865 if(Intersection(&Intersect, Env->Object, &Ray))
866 {
867 VLength(speed, Vel_Array[Index]);
868 VDist(dist, Intersect.IPoint, Pos_Array[Index]);
869 // this does only make sense for single step (euler) integration method
870 // but it could lead to agreeable result with other methods too
871 if (dist < (MechsimOptions.Time_Step*speed + MechsimOptions.Data.Masses[Index].Radius))
872 {
873 Normal(Norm, Intersect.Object, &Intersect);
874 VNormalizeEq(Norm);
875
876 /* now we do the bad thing and modify the velocity */
877 VDot(Norm_Len, Vel_Array[Index], Norm);
878 Norm_Len = -Norm_Len;
879 VAddScaledEq(Vel_Array[Index], Norm_Len, Norm); // make movement perpendicular to surface zero
880 VScaleEq(Vel_Array[Index], 1.0-Env->Friction); // apply friction
881 VAddScaledEq(Vel_Array[Index], Norm_Len*Env->Damping, Norm);
882
883 Increase_Counter(stats[MechSim_Environment_Collisions]);
884 }
885 }
886
887 }
888
889 }
890
891 /*****************************************************************************
892 *
893 * FUNCTION
894 *
895 * Mechsim_Gradient_Collide_Environment
896 *
897 * INPUT
898 *
899 * Index - index of the mass
900 * Pos_Array - state of the masses
901 * Vel_Array - *** Different usage here ***
902 * stores the last position of the mass
903 * Time - current time index for the environment function
904 * Env - Pointer to Environment
905 *
906 * OUTPUT
907 *
908 * calculated gradients of the potential fields are added to 'Gradient'.
909 *
910 * RETURNS
911 *
912 * AUTHOR
913 *
914 * Christoph Hormann
915 *
916 * DESCRIPTION
917 *
918 * Variation of Mechsim_Force_Collide_Environment() for use with gradient
919 * descent method. No damping forces.
920 *
921 * CHANGES
922 *
923 * --- Sep 2002 : Creation
924 * Oct 2002 : added object intersection based method
925 *
926 ******************************************************************************/
927
Mechsim_Gradient_Collide_Environment(int Index,VECTOR Gradient,VECTOR * Pos_Array,VECTOR * Vel_Array,DBL Time,MECHSIM_ENVIRONMENT * Env)928 void Mechsim_Gradient_Collide_Environment(int Index, VECTOR Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env)
929 {
930 DBL Potential, Fn_Val;
931 VECTOR pot_force, Grad;
932
933 if (Env->Function != NULL)
934 {
935 FunctionCode *f = POVFPU_GetFunction(*Env->Function);
936
937 // ---- collision test ----
938 POVFPU_SetLocal(X, Pos_Array[Index][X]);
939 POVFPU_SetLocal(Y, Pos_Array[Index][Y]);
940 POVFPU_SetLocal(Z, Pos_Array[Index][Z]);
941 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, Time); // additional time parameter
942
943 Fn_Val = POVFPU_Run(*Env->Function);
944
945 if (Fn_Val < MechsimOptions.Data.Masses[Index].Radius)
946 {
947 Function_Gradient( *Env->Function, Pos_Array[Index], Grad);
948 VNormalizeEq(Grad);
949
950 // --- potential force ---
951 Potential = max(0.0, MechsimOptions.Data.Masses[Index].Radius - Fn_Val);
952 VScale(pot_force, Grad, Potential*Env->Stiffness);
953
954 VAddEq(Gradient, pot_force);
955
956 Increase_Counter(stats[MechSim_Environment_Collisions]);
957 }
958 }
959 else if (Env->Object != NULL) // ======== if only object is given use intersection test ===========
960 {
961 INTERSECTION Intersect;
962 RAY Ray;
963 VECTOR Dir, Norm;
964 DBL Len;
965
966 Initialize_Ray_Containers(&Ray);
967
968 // Vel_Array[Index] stores the position from the last step so:
969 // direction = Pos_Array[Index] - Vel_Array[Index]
970 // see also: Mechsim_Simulate()
971 VSub(Dir, Pos_Array[Index], Vel_Array[Index]);
972
973 if ((Dir[X] != 0.0) || (Dir[Y] != 0.0) || (Dir[Z] != 0.0))
974 {
975 Assign_Vector(Ray.Direction, Dir);
976 Assign_Vector(Ray.Initial, Pos_Array[Index]);
977
978 if(Intersection(&Intersect, Env->Object, &Ray))
979 {
980 Normal(Norm, Intersect.Object, &Intersect); // get the normal
981 VNormalizeEq(Norm);
982 VSub(Dir, Intersect.IPoint, Pos_Array[Index]); // approximate the distance (assuming flat surface)
983 VDot(Len, Norm, Dir);
984
985 // see if we need to invert the normal
986 if (Len<0)
987 {
988 VScaleEq(Norm, -1);
989 Len = -Len;
990 }
991
992 if (Len < MechsimOptions.Data.Masses[Index].Radius)
993 {
994 // --- potential force ---
995 Potential = min(0.0, Len - MechsimOptions.Data.Masses[Index].Radius);
996 VScale(pot_force, Norm, Potential*Env->Stiffness / MechsimOptions.Data.Masses[Index].Mass);
997
998 VAddEq(Gradient, pot_force);
999
1000 Increase_Counter(stats[MechSim_Environment_Collisions]);
1001
1002 }
1003 }
1004 }
1005
1006 }
1007
1008 }
1009
1010 /*****************************************************************************
1011 *
1012 * FUNCTION
1013 *
1014 * Mechsim_Force_Collide_Environment
1015 *
1016 * INPUT
1017 *
1018 * Index - index of the mass
1019 * Pos_Array, Vel_Array - state of the masses
1020 * Time - current time index for the environment function
1021 * Env - Pointer to Environment
1022 *
1023 * OUTPUT
1024 *
1025 * calculated accelerations are added to 'Accel'.
1026 *
1027 * RETURNS
1028 *
1029 * AUTHOR
1030 *
1031 * Christoph Hormann
1032 *
1033 * DESCRIPTION
1034 *
1035 * Calculates the forces resulting from a collision with the environment.
1036 *
1037 * * If an environment function is given, it is evaluated and if
1038 * collision is found the gradient is calculated and an elastic force
1039 * is applied in direction of the gradient of the environment field.
1040 * * If only an object is given a ray is shot from the current position
1041 * in movement direction to determine the collision.
1042 *
1043 * Damping and Friction are applied, Friction already slightly above the
1044 * surface to avoid gliding.
1045 *
1046 * ========== WARNING ====================================================
1047 *
1048 * The object based method is problematic because it relies on the velocity
1049 * vector to determine the direction of the ray although the masses are
1050 * slowed down during collision and therefore the velocity will no more
1051 * be suited for a successful collision test. It's usually better to use
1052 * the impact based method instead if no function is available. Apart
1053 * from that it won't successfully find the intersection in all situations
1054 * like the function based method.
1055 *
1056 * =======================================================================
1057 *
1058 * CHANGES
1059 *
1060 * --- Aug 2002 : Creation
1061 * Oct 2002 : added object intersection based method
1062 *
1063 ******************************************************************************/
1064
Mechsim_Force_Collide_Environment(int Index,VECTOR Accel,VECTOR * Pos_Array,VECTOR * Vel_Array,DBL Time,MECHSIM_ENVIRONMENT * Env)1065 void Mechsim_Force_Collide_Environment(int Index, VECTOR Accel,VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env)
1066 {
1067 DBL Potential, Force, Fn_Val, Len;
1068 VECTOR pot_force, Grad;
1069 VECTOR FFrict, damp_force;
1070
1071 if (Env->Function != NULL) // ======== if function is given use it to calculate gradient ===========
1072 {
1073 FunctionCode *f = POVFPU_GetFunction(*Env->Function);
1074
1075 // ---- collision test ----
1076 POVFPU_SetLocal(X, Pos_Array[Index][X]);
1077 POVFPU_SetLocal(Y, Pos_Array[Index][Y]);
1078 POVFPU_SetLocal(Z, Pos_Array[Index][Z]);
1079 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, Time); // additional time parameter
1080
1081 Fn_Val = POVFPU_Run(*Env->Function);
1082
1083 if (Fn_Val < MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess)
1084 {
1085 Function_Gradient( *Env->Function, Pos_Array[Index], Grad);
1086 VNormalizeEq(Grad);
1087
1088 // --- potential force ---
1089 Potential = max(0.0, MechsimOptions.Data.Masses[Index].Radius - Fn_Val);
1090 VScale(pot_force, Grad, Potential*Env->Stiffness / MechsimOptions.Data.Masses[Index].Mass);
1091
1092 // --- damping/friction force ---
1093 if (Env->Friction > 0.0)
1094 {
1095 // friction already slightly above the surface
1096 Force = (MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess - Fn_Val)*Env->Stiffness;
1097 Calc_Friction(FFrict, Vel_Array[Index], Env->Friction, Grad, Force);
1098 }
1099 else Make_Vector(FFrict, 0.0, 0.0, 0.0);
1100
1101 damp_force[X] = -(fabs(Grad[X])*Potential*Env->Damping*Vel_Array[Index][X] + FFrict[X])/MechsimOptions.Data.Masses[Index].Mass;
1102 damp_force[Y] = -(fabs(Grad[Y])*Potential*Env->Damping*Vel_Array[Index][Y] + FFrict[Y])/MechsimOptions.Data.Masses[Index].Mass;
1103 damp_force[Z] = -(fabs(Grad[Z])*Potential*Env->Damping*Vel_Array[Index][Z] + FFrict[Z])/MechsimOptions.Data.Masses[Index].Mass;
1104
1105 VAddEq(pot_force, damp_force);
1106 VAddEq(Accel, pot_force);
1107
1108 Increase_Counter(stats[MechSim_Environment_Collisions]);
1109 }
1110 }
1111 else if (Env->Object != NULL) // ======== if only object is given use intersection test ===========
1112 {
1113 INTERSECTION Intersect;
1114 RAY Ray;
1115 VECTOR Dir, Norm;
1116
1117 Initialize_Ray_Containers(&Ray);
1118
1119 VNormalize(Dir, Vel_Array[Index]);
1120 Assign_Vector(Ray.Direction, Dir);
1121 Assign_Vector(Ray.Initial, Pos_Array[Index]);
1122
1123 if(Intersection(&Intersect, Env->Object, &Ray))
1124 {
1125 Normal(Norm, Intersect.Object, &Intersect); // get the normal
1126 VNormalizeEq(Norm);
1127 VSub(Dir, Intersect.IPoint, Pos_Array[Index]); // approximate the distance (assuming flat surface)
1128 VDot(Len, Norm, Dir);
1129
1130 // see if we need to invert the normal
1131 if (Len<0)
1132 {
1133 VScaleEq(Norm, -1);
1134 Len = -Len;
1135 }
1136
1137 if (Len < MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess)
1138 {
1139 // --- potential force ---
1140 Potential = min(0.0, Len - MechsimOptions.Data.Masses[Index].Radius);
1141 VScale(pot_force, Norm, Potential*Env->Stiffness / MechsimOptions.Data.Masses[Index].Mass);
1142
1143 // --- damping/friction force ---
1144 if (Env->Friction > 0.0)
1145 {
1146 // friction already slightly above the surface
1147 Force = (MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess - Len)*Env->Stiffness;
1148 Calc_Friction(FFrict, Vel_Array[Index], Env->Friction, Norm, Force);
1149 }
1150 else Make_Vector(FFrict, 0.0, 0.0, 0.0);
1151
1152 damp_force[X] = -(fabs(Norm[X])*Potential*Env->Damping*Vel_Array[Index][X] + FFrict[X])/MechsimOptions.Data.Masses[Index].Mass;
1153 damp_force[Y] = -(fabs(Norm[Y])*Potential*Env->Damping*Vel_Array[Index][Y] + FFrict[Y])/MechsimOptions.Data.Masses[Index].Mass;
1154 damp_force[Z] = -(fabs(Norm[Z])*Potential*Env->Damping*Vel_Array[Index][Z] + FFrict[Z])/MechsimOptions.Data.Masses[Index].Mass;
1155
1156 VAddEq(pot_force, damp_force);
1157 VAddEq(Accel, pot_force);
1158
1159 Increase_Counter(stats[MechSim_Environment_Collisions]);
1160 }
1161 }
1162
1163 }
1164
1165 }
1166
1167
1168 /*****************************************************************************
1169 *
1170 * FUNCTION
1171 *
1172 * Mechsim_Collide_Dynamic
1173 *
1174 * INPUT
1175 *
1176 * Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
1177 *
1178 * OUTPUT
1179 *
1180 * calculated accelerations are added to 'Accel'.
1181 *
1182 * RETURNS
1183 *
1184 * AUTHOR
1185 *
1186 * Christoph Hormann
1187 *
1188 * DESCRIPTION
1189 *
1190 * Calculates the forces resulting from point mass collisions
1191 *
1192 * For all mass pairs in collision elastic and damping force are calculated
1193 * and added to the 'Accel' vector of both masses.
1194 *
1195 * CHANGES
1196 *
1197 * --- Aug 2002 : Creation
1198 * Sep 2002 : Modification for grouping masses for faster simulation
1199 * Jan 2003 : Added connection-connection collisions, a lot of cleanup
1200 * Sep 2004 : Unified Gradient and Force version
1201 *
1202 ******************************************************************************/
1203
Mechsim_Collide_Dynamic(VECTOR * Accel,VECTOR * Pos_Array,VECTOR * Vel_Array,MECHSIM_BOUNDING_DATA * Bounding_Data)1204 void Mechsim_Collide_Dynamic(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
1205 {
1206 int i, j, k;
1207 int Grp1, Grp2;
1208 int Idx1, Idx2;
1209 int Start_Group, End_Group;
1210 DBL Unit;
1211
1212 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_BBOX)
1213 calc_mass_groups_bounding_box(Pos_Array, Bounding_Data);
1214
1215 /* ================================================================================== */
1216 /* mass-mass collisions */
1217 /* ================================================================================== */
1218 if (MechsimOptions.Calc_Collision != MECHSIM_COLLISION_NONE)
1219 {
1220 /* === plain collision testing (groups) === */
1221 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_NO)
1222 {
1223 if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) Start_Group=0;
1224 else Start_Group=1;
1225
1226 for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
1227 {
1228 if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) End_Group=Grp1;
1229 else End_Group=Grp1-1;
1230
1231 for (Grp2=0; Grp2<=End_Group; Grp2++)
1232 {
1233 /*
1234 This double loop accesses each combination of
1235 two groups that need to be tested against each other once.
1236 If (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL)
1237 Grp1 and Grp2 can be identical (for testing collisions within each group)
1238 */
1239 for (i=0; i<MechsimOptions.Data.Groups[Grp1].mass_count; i++)
1240 for (j=0; j<MechsimOptions.Data.Groups[Grp2].mass_count; j++)
1241 {
1242 /*
1243 Indices of the masses in conventional numbering,
1244 'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
1245 'MechsimOptions.Data.Masses[Idx1].Mass' are identical
1246 so this won't be necessary for the properties, but
1247 Vel_Array, Pos_Array and Accel need it.
1248 */
1249 Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
1250 Idx2 = MechsimOptions.Data.Groups[Grp2].Masses[j]->Index;
1251
1252 if (Idx1!=Idx2) /* avoid mass colliding with itself */
1253 {
1254 Mechsim_Collide_Mass_Mass(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1255 }
1256 } /* mass for loop */
1257 } /* group for loop */
1258 }
1259 }
1260
1261 /* === new bounding group version === */
1262 else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_BBOX)
1263 {
1264 if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) Start_Group=0;
1265 else Start_Group=1;
1266
1267 for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
1268 {
1269 if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) End_Group=Grp1;
1270 else End_Group=Grp1-1;
1271
1272 for (Grp2=0; Grp2<=End_Group; Grp2++)
1273 {
1274 /* We just check if the two groups intersect. */
1275 if (intersect_box_box(Bounding_Data->Mass_Groups_BBox_Array[Grp1],
1276 Bounding_Data->Mass_Groups_BBox_Array[Grp2]))
1277 {
1278 calc_mass_group_intersection(Pos_Array,
1279 Bounding_Data->Mass_Groups_BBox_Array[Grp1], Grp2, Bounding_Data);
1280 for (i=0; i<MechsimOptions.Data.Groups[Grp1].mass_count; i++)
1281 {
1282 /*
1283 Indices of the masses in conventional numbering,
1284 'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
1285 'MechsimOptions.Data.Masses[Idx1].Mass' are identical
1286 so this won't be necessary for the properties, but
1287 Vel_Array, Pos_Array and Accel need it.
1288 */
1289 Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
1290 if (intersect_sphere_box(Bounding_Data->Mass_Groups_BBox_Array[Grp2],
1291 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius))
1292 {
1293 for (j = 0; j < Bounding_Data->intersect_list_index; j++)
1294 {
1295 Idx2 = MechsimOptions.Data.Groups[Grp2].Masses[j]->Index;
1296 if (Idx1!=Idx2) /* avoid mass colliding with itself */
1297 {
1298 Mechsim_Collide_Mass_Mass(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1299 }
1300 }
1301 }
1302 } /* mass for loop */
1303 }
1304 } /* group for loop */
1305 }
1306 }
1307 /* === hash table version === */
1308 else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
1309 {
1310 Unit = calc_mass_unit_size();
1311 Make_Vector(Bounding_Data->Unit, Unit, Unit, Unit);
1312
1313 calc_masses_bounding_hashtable(Pos_Array, Bounding_Data);
1314
1315 for (Idx1 = 0; Idx1 < MechsimOptions.Data.mass_count; Idx1++)
1316 /* loop through all masses */
1317 {
1318 intersect_sphere_bounding_hashtable(Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius, Bounding_Data);
1319 for (k = 0; k < Bounding_Data->intersect_list_index; k++)
1320 {
1321 Idx2 = Bounding_Data->intersect_list[k];
1322 if (Idx1 != Idx2)
1323 {
1324 Mechsim_Collide_Mass_Mass(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1325 }
1326 } /* mass for loop */
1327 } /* mass for loop */
1328 }
1329 }
1330
1331 /* ================================================================================== */
1332 /* mass-face collisions */
1333 /* ================================================================================== */
1334 if ((MechsimOptions.Calc_Collision_Faces != MECHSIM_COLLISION_NONE) && (MechsimOptions.Data.faces_count>0))
1335 {
1336 /* === plain collision testing (groups) === */
1337 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_NO)
1338 {
1339 for (Grp1=0; Grp1<MechsimOptions.Data.group_count; Grp1++)
1340 for (Grp2=0; Grp2<MechsimOptions.Data.group_count; Grp2++)
1341 if (
1342 (MechsimOptions.Calc_Collision_Faces == MECHSIM_COLLISION_ALL) ||
1343 (Grp1 != Grp2)
1344 )
1345 {
1346 /*
1347 This double loop accesses each combination of
1348 two groups that need to be tested against each other *TWICE*
1349 (in contrast to the mass mass collision. One time for testing the
1350 faces of Grp1 with the masses of Grp2, the other vica verse.
1351 If (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL)
1352 Grp1 and Grp2 can be identical (for testing collisions within each group)
1353 */
1354 for (i=0; i<MechsimOptions.Data.Groups[Grp1].mass_count; i++)
1355 for (j=0; j<MechsimOptions.Data.Groups[Grp2].faces_count; j++)
1356 {
1357 /*
1358 Indices of the masses and faces in conventional numbering,
1359 'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
1360 'MechsimOptions.Data.Masses[Idx1].Mass' are identical
1361 so this won't be necessary for the properties, but
1362 Vel_Array, Pos_Array and Accel need it.
1363 */
1364 Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
1365 Idx2 = MechsimOptions.Data.Groups[Grp2].Faces[j]->Index;
1366
1367 if (
1368 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx1) &&
1369 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx2) &&
1370 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx3)
1371 /* avoid testing against a face the mass i belongs to */
1372 )
1373 {
1374 Mechsim_Collide_Mass_Face(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1375 }
1376 } /* mass for loop */
1377 } /* group for loop */
1378 }
1379 /* === new bounding group version === */
1380 else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_BBOX)
1381 {
1382 calc_face_groups_bounding_box(Pos_Array, Bounding_Data);
1383
1384 for (Grp1=0; Grp1<MechsimOptions.Data.group_count; Grp1++)
1385 for (Grp2=0; Grp2<MechsimOptions.Data.group_count; Grp2++)
1386
1387 if (
1388 (MechsimOptions.Calc_Collision_Faces == MECHSIM_COLLISION_ALL) ||
1389 ((Grp1 != Grp2) &&
1390 intersect_box_box(Bounding_Data->Mass_Groups_BBox_Array[Grp1],
1391 Bounding_Data->Face_Groups_BBox_Array[Grp2])
1392 )
1393 )
1394 {
1395 calc_face_group_intersection(Bounding_Data->Mass_Groups_BBox_Array[Grp1], Grp2, Bounding_Data);
1396 /*
1397 This double loop accesses each combination of
1398 two groups that need to be tested against each other *TWICE*
1399 (in contrast to the mass mass collision. One time for testing the
1400 faces of Grp1 with the masses of Grp2, the other vica verse.
1401 If (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL)
1402 Grp1 and Grp2 can be identical (for testing collisions within each group)
1403 */
1404 for (i = 0; i < MechsimOptions.Data.Groups[Grp1].mass_count; i++)
1405 {
1406 Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
1407 /* We just check if the mass intersects the second group. */
1408 if (intersect_sphere_box(Bounding_Data->Face_Groups_BBox_Array[Grp2],
1409 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius))
1410 {
1411 for (j = 0; j < Bounding_Data->intersect_list_index; j++)
1412 {
1413 /*
1414 Indices of the masses and faces in conventional numbering,
1415 'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
1416 'MechsimOptions.Data.Masses[Idx1].Mass' are identical
1417 so this won't be necessary for the properties, but
1418 Vel_Array, Pos_Array and Accel need it.
1419 */
1420 Idx2 = Bounding_Data->intersect_list[j];
1421
1422 if (
1423 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx1) &&
1424 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx2) &&
1425 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx3) && /* avoid testing against a face the mass i belongs to */
1426 intersect_sphere_box(Bounding_Data->Faces_BBox_Array[Idx2],
1427 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius)
1428 /* quick check if mass intersects face */
1429 )
1430 {
1431 Mechsim_Collide_Mass_Face(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1432 }
1433 }
1434 }
1435 }
1436 }
1437 }
1438 /* === hash table version === */
1439 else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
1440 {
1441 calc_faces_bounding_box(Pos_Array, Bounding_Data);
1442 calc_faces_bounding_hashtable(Bounding_Data);
1443
1444 for (Idx1 = 0; Idx1 < MechsimOptions.Data.mass_count; Idx1++)
1445 /* loop through all masses */
1446 {
1447 intersect_sphere_bounding_hashtable(Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius, Bounding_Data);
1448 for (k = 0; k < Bounding_Data->intersect_list_index; k++)
1449 {
1450 Idx2 = Bounding_Data->intersect_list[k];
1451 if (
1452 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx1) &&
1453 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx2) &&
1454 (Idx1 != MechsimOptions.Data.Faces[Idx2].Idx3) && /* avoid testing against a face the mass i belongs to */
1455 (
1456 (MechsimOptions.Calc_Collision_Faces == MECHSIM_COLLISION_ALL) ||
1457 (MechsimOptions.Data.Faces[Idx2].Group != MechsimOptions.Data.Masses[Idx1].Group)
1458 ) &&
1459 /* and make sure mass and face are in different groups */
1460 intersect_sphere_box(Bounding_Data->Faces_BBox_Array[Idx2],
1461 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius)
1462 )
1463 {
1464 Mechsim_Collide_Mass_Face(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1465 }
1466 } /* face for loop */
1467 } /* mass for loop */
1468 }
1469 }
1470
1471 /* ================================================================================== */
1472 /* connection-connection collisions */
1473 /* ================================================================================== */
1474 if ((MechsimOptions.Calc_Collision_Connections != MECHSIM_COLLISION_NONE) && (MechsimOptions.Data.connect_count>0))
1475 {
1476 /* === plain collision testing (groups) === */
1477 if (MechsimOptions.Bounding < MECHSIM_FACE_BOUNDING_BBOX)
1478 {
1479 if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) Start_Group=0;
1480 else Start_Group=1;
1481
1482 for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
1483 {
1484 if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) End_Group=Grp1;
1485 else End_Group=Grp1-1;
1486
1487 for (Grp2=0; Grp2<=End_Group; Grp2++)
1488 {
1489 /*
1490 This double loop accesses each combination of
1491 two groups that need to be tested against each other once (just like in
1492 mass-mass-collisions).
1493 If (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL)
1494 Grp1 and Grp2 can be identical (for testing collisions within each group)
1495 */
1496 for (i=0; i<MechsimOptions.Data.Groups[Grp1].connect_count; i++)
1497 for (j=0; j<MechsimOptions.Data.Groups[Grp2].connect_count; j++)
1498 {
1499 /*
1500 Indices of the connections in conventional numbering,
1501 see mass-mass-collisions for more details.
1502 */
1503 Idx1 = MechsimOptions.Data.Groups[Grp1].Connects[i]->Index;
1504 Idx2 = MechsimOptions.Data.Groups[Grp2].Connects[j]->Index;
1505
1506 if ((Idx1!=Idx2) && /* avoid connection colliding with itself */
1507 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
1508 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1) &&
1509 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
1510 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
1511 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
1512 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
1513 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
1514 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1))
1515 /* and also do not test connections with common masses */
1516 {
1517 Mechsim_Collide_Connection_Connection(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1518 }
1519 } /* connection for loop */
1520 } /* group for loop */
1521 }
1522 }
1523 /* === new bounding group version === */
1524 else
1525 {
1526 calc_connect_groups_bounding_box(Pos_Array, Bounding_Data);
1527 if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) Start_Group=0;
1528 else Start_Group=1;
1529
1530 for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
1531 {
1532 if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) End_Group=Grp1;
1533 else End_Group=Grp1-1;
1534
1535 for (Grp2=0; Grp2<=End_Group; Grp2++)
1536 {
1537 /* We just check if the two groups intersect. */
1538 if (intersect_box_box(Bounding_Data->Connect_Groups_BBox_Array[Grp1],
1539 Bounding_Data->Connect_Groups_BBox_Array[Grp2]))
1540 {
1541 calc_connect_group_intersection(Pos_Array, Bounding_Data->Connect_Groups_BBox_Array[Grp1], Grp2, Bounding_Data);
1542 for (i = 0; i < MechsimOptions.Data.Groups[Grp1].connect_count; i++)
1543 {
1544 Idx1 = MechsimOptions.Data.Groups[Grp1].Connects[i]->Index;
1545 /* We just check if the connection intersects the second group. */
1546 if (intersect_box_box(Bounding_Data->Connect_Groups_BBox_Array[Grp2],
1547 Bounding_Data->Connects_BBox_Array[Idx1]))
1548 {
1549 for (j = 0; j < Bounding_Data->intersect_list_index; j++)
1550 {
1551 /*
1552 Indices of the masses and faces in conventional numbering,
1553 'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
1554 'MechsimOptions.Data.Masses[Idx1].Mass' are identical
1555 so this won't be necessary for the properties, but
1556 Vel_Array, Pos_Array and Accel need it.
1557 */
1558 Idx2 = Bounding_Data->intersect_list[j];
1559
1560 if ((Idx1!=Idx2) && /* avoid connection colliding with itself */
1561 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
1562 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1) &&
1563 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
1564 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
1565 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
1566 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
1567 (MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
1568 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1) &&
1569 /* and also do not test connections with common masses */
1570 intersect_box_box(Bounding_Data->Connects_BBox_Array[Idx1],
1571 Bounding_Data->Connects_BBox_Array[Idx2]))
1572 {
1573 Mechsim_Collide_Connection_Connection(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
1574 }
1575 }
1576 } /* connection for loop */
1577 }
1578 }
1579 } /* group for loop */
1580 }
1581 }
1582 }
1583 }
1584
1585 /*****************************************************************************
1586 *
1587 * FUNCTION
1588 *
1589 * Mechsim_Collide_Mass_Mass
1590 *
1591 * INPUT
1592 *
1593 * Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
1594 * Idx1, Idx2 - index of the masses
1595 *
1596 * OUTPUT
1597 *
1598 * calculated accelerations are added to 'Accel'.
1599 *
1600 * RETURNS
1601 *
1602 * AUTHOR
1603 *
1604 * Christoph Hormann
1605 *
1606 * DESCRIPTION
1607 *
1608 * Calculates the forces resulting from mass-mass collisions
1609 *
1610 *
1611 * CHANGES
1612 *
1613 * --- Aug 2002 : Creation
1614 * Sep 2004 : moved to separate function by Daniel Jungmann
1615 *
1616 ******************************************************************************/
Mechsim_Collide_Mass_Mass(VECTOR * Accel,VECTOR * Pos_Array,VECTOR * Vel_Array,int Idx1,int Idx2)1617 void Mechsim_Collide_Mass_Mass(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2)
1618 {
1619 DBL Dist, lAxis;
1620 VECTOR Axis, Vel;
1621 VECTOR damp_force;
1622 VECTOR pot_force, FFrict;
1623 DBL Force;
1624 DBL Potential;
1625
1626 VSub(Axis, Pos_Array[Idx1], Pos_Array[Idx2]);
1627 VLength(lAxis, Axis);
1628 Dist = MechsimOptions.Data.Masses[Idx1].Radius+MechsimOptions.Data.Masses[Idx2].Radius;
1629
1630 if (lAxis < Dist*MechsimOptions.Collision_Friction_Excess)
1631 {
1632 VInverseScaleEq(Axis, lAxis);
1633
1634 // --- potential force ---
1635 Potential = max(0.0, Dist-lAxis);
1636 VScale(pot_force, Axis, Potential*MechsimOptions.Collision_Stiffness);
1637
1638 // Force version (with damping and mass)
1639 if (Vel_Array != NULL)
1640 {
1641 // --- damping/friction force ---
1642 VSub(Vel, Vel_Array[Idx1], Vel_Array[Idx2]); // relative velocity
1643
1644 if (MechsimOptions.Collision_Friction > 0.0)
1645 {
1646 // friction already slightly above the surface
1647 Force = (Dist*MechsimOptions.Collision_Friction_Excess - lAxis)*MechsimOptions.Collision_Stiffness;
1648 Calc_Friction(FFrict, Vel, MechsimOptions.Collision_Friction, Axis, Force);
1649 }
1650 else Make_Vector(FFrict, 0.0, 0.0, 0.0);
1651
1652 damp_force[X] = -(fabs(Axis[X])*Potential*MechsimOptions.Collision_Damping*Vel[X] + FFrict[X]);
1653 damp_force[Y] = -(fabs(Axis[Y])*Potential*MechsimOptions.Collision_Damping*Vel[Y] + FFrict[Y]);
1654 damp_force[Z] = -(fabs(Axis[Z])*Potential*MechsimOptions.Collision_Damping*Vel[Z] + FFrict[Z]);
1655
1656 VAddEq(pot_force, damp_force);
1657
1658 VAddScaledEq(Accel[Idx1], 1.0/MechsimOptions.Data.Masses[Idx1].Mass, pot_force);
1659 VAddScaledEq(Accel[Idx2], -1.0/MechsimOptions.Data.Masses[Idx1].Mass, pot_force);
1660 }
1661 // Gradient version
1662 else
1663 {
1664 VAddEq(Accel[Idx1], pot_force);
1665 VSubEq(Accel[Idx2], pot_force);
1666 }
1667
1668 Increase_Counter(stats[MechSim_Mass_Collisions]);
1669 }
1670 }
1671
1672 /*****************************************************************************
1673 *
1674 * FUNCTION
1675 *
1676 * Mechsim_Collide_Mass_Face
1677 *
1678 * INPUT
1679 *
1680 * Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
1681 * Idx1, Idx2 - index of the mass and face
1682 *
1683 * OUTPUT
1684 *
1685 * calculated accelerations are added to 'Accel'.
1686 *
1687 * RETURNS
1688 *
1689 * AUTHOR
1690 *
1691 * Christoph Hormann
1692 *
1693 * DESCRIPTION
1694 *
1695 * Calculates the forces resulting from mass-face collisions
1696 *
1697 *
1698 * CHANGES
1699 *
1700 * --- Aug 2002 : Creation
1701 * Sep 2004 : moved to separate function by Daniel Jungmann
1702 *
1703 ******************************************************************************/
Mechsim_Collide_Mass_Face(VECTOR * Accel,VECTOR * Pos_Array,VECTOR * Vel_Array,int Idx1,int Idx2)1704 void Mechsim_Collide_Mass_Face(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2)
1705 {
1706 VECTOR Grad;
1707 VECTOR CPoint;
1708 VECTOR damp_force;
1709 VECTOR pot_force, FFrict;
1710 VECTOR Vel;
1711 DBL Force;
1712 DBL Potential;
1713 DBL Fn_Val, Ratio;
1714 DBL Weight1, Weight2, Weight3;
1715 int Side;
1716
1717 // ---- collision test ----
1718 // also calculates contact point and gradient
1719 Side = func_triangle(Pos_Array[Idx1], CPoint, Grad,
1720 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx1],
1721 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx2],
1722 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx3], &Ratio);
1723
1724 VLength(Fn_Val, Grad);
1725
1726 if (Fn_Val < MechsimOptions.Data.Masses[Idx1].Radius*MechsimOptions.Collision_Friction_Excess)
1727 {
1728 VInverseScaleEq(Grad, Fn_Val);
1729
1730 // --- now calculate distribution of reaction force between triangle vertices ---
1731
1732 switch (Side)
1733 {
1734 // if outside, the triangle function has already returned the nearest side
1735 case 1: /* P1-P2 */
1736 Weight1 = 1.0-Ratio;
1737 Weight2 = Ratio;
1738 Weight3 = 0.0;
1739 break;
1740 case 2: /* P2-P3 */
1741 Weight2 = 1.0-Ratio;
1742 Weight3 = Ratio;
1743 Weight1 = 0.0;
1744 break;
1745 case 3: /* P3-P1 */
1746 Weight3 = 1.0-Ratio;
1747 Weight1 = Ratio;
1748 Weight2 = 0.0;
1749 break;
1750 default:
1751 // if inside triangle, calculate distribution between the three vertices
1752 Triangle_Distribute(CPoint,
1753 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx1],
1754 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx2],
1755 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx3],
1756 &Weight1, &Weight2, &Weight3);
1757 break;
1758 }
1759
1760 // --- potential force ---
1761 Potential = max(0.0, MechsimOptions.Data.Masses[Idx1].Radius - Fn_Val);
1762 VScale(pot_force, Grad, Potential*MechsimOptions.Collision_Stiffness);
1763
1764 // Force version (with damping and mass)
1765 if (Vel_Array != NULL)
1766 {
1767 // --- damping/friction force ---
1768 Vel[X] = Vel_Array[Idx1][X] - (1.0/3.0)*(Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx1][X]+
1769 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx2][X]+
1770 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx3][X]);
1771 Vel[Y] = Vel_Array[Idx1][Y] - (1.0/3.0)*(Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx1][Y]+
1772 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx2][Y]+
1773 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx3][Y]);
1774 Vel[Z] = Vel_Array[Idx1][Z] - (1.0/3.0)*(Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx1][Z]+
1775 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx2][Z]+
1776 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx3][Z]); // relative velocity
1777
1778 if (MechsimOptions.Collision_Friction > 0.0)
1779 {
1780 // friction already slightly above the surface
1781 Force = (MechsimOptions.Data.Masses[Idx1].Radius*MechsimOptions.Collision_Friction_Excess - Fn_Val)*MechsimOptions.Collision_Stiffness;
1782 Calc_Friction(FFrict, Vel, MechsimOptions.Collision_Friction, Grad, Force);
1783 }
1784 else Make_Vector(FFrict, 0.0, 0.0, 0.0);
1785
1786 damp_force[X] = -(fabs(Grad[X])*Potential*MechsimOptions.Collision_Damping*Vel[X] + FFrict[X]);
1787 damp_force[Y] = -(fabs(Grad[Y])*Potential*MechsimOptions.Collision_Damping*Vel[Y] + FFrict[Y]);
1788 damp_force[Z] = -(fabs(Grad[Z])*Potential*MechsimOptions.Collision_Damping*Vel[Z] + FFrict[Z]);
1789
1790 VAddEq(pot_force, damp_force); // apply to mass
1791 VAddScaledEq(Accel[Idx1], 1.0/MechsimOptions.Data.Masses[Idx1].Mass, pot_force);
1792
1793 // apply forces to the vertices
1794 VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx1],
1795 -Weight1/MechsimOptions.Data.Masses[MechsimOptions.Data.Faces[Idx2].Idx1].Mass,
1796 pot_force);
1797 VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx2],
1798 -Weight2/MechsimOptions.Data.Masses[MechsimOptions.Data.Faces[Idx2].Idx2].Mass,
1799 pot_force);
1800 VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx3],
1801 -Weight3/MechsimOptions.Data.Masses[MechsimOptions.Data.Faces[Idx2].Idx3].Mass,
1802 pot_force);
1803
1804 }
1805 // Gradient version
1806 else
1807 {
1808 VAddEq(Accel[Idx1], pot_force);
1809
1810 // apply forces to the vertices
1811 VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx1], -Weight1, pot_force);
1812 VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx2], -Weight2, pot_force);
1813 VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx3], -Weight3, pot_force);
1814 }
1815
1816 Increase_Counter(stats[MechSim_Face_Collisions]);
1817
1818 }
1819 }
1820
1821 /*****************************************************************************
1822 *
1823 * FUNCTION
1824 *
1825 * Mechsim_Collide_Mass_Face
1826 *
1827 * INPUT
1828 *
1829 * Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
1830 * Idx1, Idx2 - index of the connections
1831 *
1832 * OUTPUT
1833 *
1834 * calculated accelerations are added to 'Accel'.
1835 *
1836 * RETURNS
1837 *
1838 * AUTHOR
1839 *
1840 * Christoph Hormann
1841 *
1842 * DESCRIPTION
1843 *
1844 * Calculates the forces resulting from connection-connection collisions
1845 *
1846 *
1847 * CHANGES
1848 *
1849 * --- Aug 2002 : Creation
1850 * Sep 2004 : moved to separate function by Daniel Jungmann
1851 *
1852 ******************************************************************************/
Mechsim_Collide_Connection_Connection(VECTOR * Accel,VECTOR * Pos_Array,VECTOR * Vel_Array,int Idx1,int Idx2)1853 void Mechsim_Collide_Connection_Connection(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2)
1854 {
1855 VECTOR C, D, R1, R2;
1856 VECTOR damp_force;
1857 VECTOR pot_force, FFrict;
1858 VECTOR Vel;
1859 DBL Dist, lAxis;
1860 DBL Force;
1861 DBL Potential;
1862 DBL Fact, afact, bfact;
1863 int Dir1, Dir2, Dir3;
1864
1865 /*
1866 for connection-connection collisions we need to find the distance
1867 between two straight lines defined by the connections and have to
1868 test if this smallest distance is less than the sum of the radii
1869 and if it occurs between the end points of the connections. The
1870 problem can be described by the (vector) equation:
1871
1872 P1 + alpha*(P2-P1) + gamma*D = Q1 + beta*(Q2-Q1)
1873
1874 where D = vnormalize(vcross((P2-P1), (Q2-Q1))).
1875
1876 gamma is the distance between the lines, alpha and beta have to
1877 be between 0 and 1. Further symbols used in the code:
1878
1879 R1 = (P2-P1)
1880 R2 = (Q2-Q1)
1881 C = (Q1-P1)
1882 */
1883
1884 VSub(R1, Pos_Array[MechsimOptions.Data.Connects[Idx1].Idx2],
1885 Pos_Array[MechsimOptions.Data.Connects[Idx1].Idx1] );
1886 VSub(R2, Pos_Array[MechsimOptions.Data.Connects[Idx2].Idx2],
1887 Pos_Array[MechsimOptions.Data.Connects[Idx2].Idx1] );
1888 VCross(D, R1, R2);
1889
1890 VLength(Dist, D);
1891
1892 if (Dist>MECHSIM_EPSILON) /* the connection lines must not be collinear */
1893 {
1894 VInverseScaleEq(D, Dist);
1895
1896 if (fabs(R1[X])>fabs(R1[Y])) /* Sort the Coordinates to avoid div by zero */
1897 {
1898 if (fabs(R1[X])>fabs(R1[Z])) { Dir1 = X; Dir2 = Y; Dir3 = Z; }
1899 else { Dir1 = Z; Dir2 = Y; Dir3 = X; }
1900 }
1901 else
1902 {
1903 if (fabs(R1[Y])>fabs(R1[Z])) { Dir1 = Y; Dir2 = X; Dir3 = Z; }
1904 else { Dir1 = Z; Dir2 = Y; Dir3 = X; }
1905 }
1906
1907 if (fabs(R1[Dir1])>MECHSIM_EPSILON)
1908 {
1909 Fact = (-R2[Dir2] + R2[Dir1]*(R1[Dir2]/R1[Dir1]));
1910 if (Fact > MECHSIM_EPSILON)
1911 {
1912 Fact = (-R2[Dir3] + R2[Dir1]*(R1[Dir3]/R1[Dir1])) / Fact;
1913
1914 lAxis = (D[Dir3] - D[Dir1]*(R1[Dir3]/R1[Dir1]) - (D[Dir2]-D[Dir1]*(R1[Dir2]/R1[Dir1]))*Fact);
1915
1916 if (lAxis > MECHSIM_EPSILON)
1917 {
1918 VSub(C, Pos_Array[MechsimOptions.Data.Connects[Idx2].Idx1],
1919 Pos_Array[MechsimOptions.Data.Connects[Idx1].Idx1]);
1920 lAxis = (C[Dir3] - C[Dir1]*(R1[Dir3]/R1[Dir1]) - (C[Dir2]-C[Dir1]*(R1[Dir2]/R1[Dir1]))*Fact) / lAxis;
1921 }
1922 else return;
1923 }
1924 else return;
1925
1926 Dist = (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx1].Radius+
1927 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx2].Radius+
1928 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx1].Radius+
1929 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx2].Radius)*0.5;
1930
1931 }
1932 else return;
1933
1934 if ((fabs(lAxis) < Dist*MechsimOptions.Collision_Friction_Excess) &&
1935 (fabs(lAxis) > MECHSIM_EPSILON))
1936 {
1937 bfact = (C[Dir2] - C[Dir1]*(R1[Dir2]/R1[Dir1]) - lAxis*(D[Dir2]-D[Dir1]*(R1[Dir2]/R1[Dir1])))/(-R2[Dir2] + R2[Dir1]*(R1[Dir2]/R1[Dir1]));
1938
1939 if ((bfact < 1.0) && (bfact > 0.0))
1940 {
1941 afact = (C[Dir1] - lAxis*D[Dir1] + bfact*R2[Dir1]) / R1[Dir1];
1942
1943 if ((afact < 1.0) && (afact > 0.0))
1944 {
1945
1946 // --- potential force ---
1947 lAxis = fabs(lAxis);
1948 Potential = max(0.0, Dist - lAxis);
1949
1950 VScale(pot_force, D, Potential*MechsimOptions.Collision_Stiffness);
1951
1952 // Force version (with damping and mass)
1953 if (Vel_Array != NULL)
1954 {
1955 // --- damping/friction force ---
1956 Vel[X] = ((Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx1][X] + Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx2][X]) -
1957 (Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx1][X] + Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx2][X]))/2.0;
1958 Vel[Y] = ((Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx1][Y] + Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx2][Y]) -
1959 (Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx1][Y] + Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx2][Y]))/2.0;
1960 Vel[Z] = ((Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx1][Z] + Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx2][Z]) -
1961 (Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx1][Z] + Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx2][Z]))/2.0;
1962 // relative velocity
1963
1964 if (MechsimOptions.Collision_Friction > 0.0)
1965 {
1966 // friction already slightly above the surface
1967 Force = (Dist*MechsimOptions.Collision_Friction_Excess - lAxis)*MechsimOptions.Collision_Stiffness;
1968 Calc_Friction(FFrict, Vel, MechsimOptions.Collision_Friction, D, Force);
1969 }
1970 else Make_Vector(FFrict, 0.0, 0.0, 0.0);
1971
1972 damp_force[X] = -(fabs(D[X])*Potential*MechsimOptions.Collision_Damping*Vel[X] + FFrict[X]);
1973 damp_force[Y] = -(fabs(D[Y])*Potential*MechsimOptions.Collision_Damping*Vel[Y] + FFrict[Y]);
1974 damp_force[Z] = -(fabs(D[Z])*Potential*MechsimOptions.Collision_Damping*Vel[Z] + FFrict[Z]);
1975
1976 VAddEq(pot_force, damp_force); // apply to masses
1977
1978 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx1],
1979 (1.0-afact)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx1].Mass, pot_force);
1980 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx2],
1981 afact/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx2].Mass, pot_force);
1982 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx1],
1983 -(1.0-bfact)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx1].Mass, pot_force);
1984 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx2],
1985 -bfact/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx2].Mass, pot_force);
1986 }
1987 // Gradient version
1988 else
1989 {
1990 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx1], (1.0-afact), pot_force);
1991 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx2], afact, pot_force);
1992 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx1], -(1.0-bfact), pot_force);
1993 VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx2], -bfact, pot_force);
1994 }
1995
1996 Increase_Counter(stats[MechSim_Connection_Collisions]);
1997 }
1998 }
1999 }
2000 }
2001 }
2002
2003 /*****************************************************************************
2004 *
2005 * FUNCTION
2006 *
2007 * Mechsim_Gradient_Connect
2008 *
2009 * INPUT
2010 *
2011 * Pos_Array - state of the masses
2012 *
2013 * OUTPUT
2014 *
2015 * calculated gradients of the potential fields are added to 'Gradient'.
2016 *
2017 * RETURNS
2018 *
2019 * AUTHOR
2020 *
2021 * Christoph Hormann
2022 *
2023 * DESCRIPTION
2024 *
2025 * Variation of Mechsim_Force_Connect() for use with gradient
2026 * descent method. No damping forces.
2027 *
2028 * CHANGES
2029 *
2030 * --- Sep 2002 : Creation
2031 *
2032 ******************************************************************************/
2033
Mechsim_Gradient_Connect(VECTOR * Gradient,VECTOR * Pos_Array)2034 void Mechsim_Gradient_Connect(VECTOR *Gradient, VECTOR *Pos_Array)
2035 {
2036 int i;
2037 DBL lAxis;
2038 VECTOR Axis;
2039 VECTOR pot_force;
2040
2041 for (i=0; i<MechsimOptions.Data.connect_count; i++)
2042 {
2043 VSub(Axis, Pos_Array[MechsimOptions.Data.Connects[i].Idx1],
2044 Pos_Array[MechsimOptions.Data.Connects[i].Idx2] );
2045 VLength(lAxis, Axis);
2046
2047 if (lAxis>0) // otherwise we can't conclude the direction
2048 {
2049 // --- potential force ---
2050 VScale(pot_force, Axis, -((lAxis-MechsimOptions.Data.Connects[i].Length)/lAxis)*MechsimOptions.Data.Connects[i].Stiffness);
2051
2052 VAddEq(Gradient[MechsimOptions.Data.Connects[i].Idx1], pot_force);
2053 VSubEq(Gradient[MechsimOptions.Data.Connects[i].Idx2], pot_force);
2054
2055 }
2056 }
2057 }
2058
2059 /*****************************************************************************
2060 *
2061 * FUNCTION
2062 *
2063 * Mechsim_Force_Connect
2064 *
2065 * INPUT
2066 *
2067 * Pos_Array, Vel_Array - state of the masses
2068 *
2069 * OUTPUT
2070 *
2071 * calculated accelerations are added to 'Accel'.
2072 *
2073 * RETURNS
2074 *
2075 * AUTHOR
2076 *
2077 * Christoph Hormann
2078 *
2079 * DESCRIPTION
2080 *
2081 * Calculates the forces resulting from connections
2082 *
2083 * Looping through the connection array all connected mass pairs are
2084 * handled and accelerations added to the 'Accel' vector of both masses.
2085 *
2086 * CHANGES
2087 *
2088 * --- Aug 2002 : Creation
2089 *
2090 ******************************************************************************/
2091
Mechsim_Force_Connect(VECTOR * Accel,VECTOR * Pos_Array,VECTOR * Vel_Array)2092 void Mechsim_Force_Connect(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array)
2093 {
2094 int i;
2095 DBL lAxis, Force_Val;
2096 VECTOR Axis;
2097 VECTOR pot_force1, pot_force2;
2098 DBL damp_fact;
2099
2100 for (i=0; i<MechsimOptions.Data.connect_count; i++)
2101 {
2102 VSub(Axis, Pos_Array[MechsimOptions.Data.Connects[i].Idx1],
2103 Pos_Array[MechsimOptions.Data.Connects[i].Idx2] );
2104 VLength(lAxis, Axis);
2105
2106 if (lAxis>0) // otherwise we can't conclude the direction
2107 {
2108 // --- potential force ---
2109 Force_Val = (lAxis-MechsimOptions.Data.Connects[i].Length)*MechsimOptions.Data.Connects[i].Stiffness;
2110
2111 // --- damping force ---
2112
2113 if (MechsimOptions.Data.Connects[i].Damping > 0.0)
2114 {
2115 damp_fact = // (x*vx + y*vy + z*vz)/ (x^2 + y^2 + z^2)
2116 (
2117 (Pos_Array[MechsimOptions.Data.Connects[i].Idx1][X]-Pos_Array[MechsimOptions.Data.Connects[i].Idx2][X])*
2118 (Vel_Array[MechsimOptions.Data.Connects[i].Idx1][X]-Vel_Array[MechsimOptions.Data.Connects[i].Idx2][X])
2119 +
2120 (Pos_Array[MechsimOptions.Data.Connects[i].Idx1][Y]-Pos_Array[MechsimOptions.Data.Connects[i].Idx2][Y])*
2121 (Vel_Array[MechsimOptions.Data.Connects[i].Idx1][Y]-Vel_Array[MechsimOptions.Data.Connects[i].Idx2][Y])
2122 +
2123 (Pos_Array[MechsimOptions.Data.Connects[i].Idx1][Z]-Pos_Array[MechsimOptions.Data.Connects[i].Idx2][Z])*
2124 (Vel_Array[MechsimOptions.Data.Connects[i].Idx1][Z]-Vel_Array[MechsimOptions.Data.Connects[i].Idx2][Z])
2125 )/(lAxis*lAxis);
2126
2127 Force_Val += damp_fact*MechsimOptions.Data.Connects[i].Damping;
2128 }
2129
2130 VScale(pot_force1, Axis, -(Force_Val/lAxis)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[i].Idx1].Mass);
2131 VScale(pot_force2, Axis, (Force_Val/lAxis)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[i].Idx2].Mass);
2132
2133 VAddEq(Accel[MechsimOptions.Data.Connects[i].Idx1], pot_force1);
2134 VAddEq(Accel[MechsimOptions.Data.Connects[i].Idx2], pot_force2);
2135
2136 }
2137 }
2138 }
2139
2140
2141 /*****************************************************************************
2142 *
2143 * FUNCTION
2144 *
2145 * Mechsim_Connect_Evaluate
2146 *
2147 * INPUT
2148 *
2149 * Idx - number of connection to evaluate
2150 *
2151 * OUTPUT
2152 *
2153 * RETURNS
2154 *
2155 * The force of the connection
2156 *
2157 * AUTHOR
2158 *
2159 * Christoph Hormann
2160 *
2161 * DESCRIPTION
2162 *
2163 * Calculates the force resulting from a connection.
2164 *
2165 * This is supposed to be called after the simulation is finished.
2166 *
2167 * CHANGES
2168 *
2169 * --- Mar 2005 : Creation
2170 *
2171 ******************************************************************************/
2172
Mechsim_Connect_Evaluate(int Idx)2173 DBL Mechsim_Connect_Evaluate(int Idx)
2174 {
2175 DBL lAxis, xForce;
2176 VECTOR Axis;
2177 DBL damp_fact;
2178
2179
2180 VSub(Axis, MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position,
2181 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position );
2182 VLength(lAxis, Axis);
2183
2184 if (lAxis>0) // otherwise we can't conclude the direction
2185 {
2186 // --- potential force ---
2187 xForce = (lAxis-MechsimOptions.Data.Connects[Idx].Length)*MechsimOptions.Data.Connects[Idx].Stiffness;
2188
2189 // --- damping force ---
2190
2191 if (MechsimOptions.Data.Connects[Idx].Damping > 0.0)
2192 {
2193 damp_fact = // (x*vx + y*vy + z*vz)/ (x^2 + y^2 + z^2)
2194 (
2195 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position[X]-
2196 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position[X])*
2197 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Velocity[X]-
2198 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Velocity[X])
2199 +
2200 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position[Y]-
2201 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position[Y])*
2202 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Velocity[Y]-
2203 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Velocity[Y])
2204 +
2205 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position[Z]-
2206 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position[Z])*
2207 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Velocity[Z]-
2208 MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Velocity[Z])
2209 )/(lAxis*lAxis);
2210
2211
2212 xForce += damp_fact*MechsimOptions.Data.Connects[Idx].Damping;
2213 }
2214
2215 }
2216 else xForce = 0.0;
2217
2218 return xForce;
2219 }
2220
2221
2222 /*****************************************************************************
2223 *
2224 * FUNCTION
2225 *
2226 * Mechsim_Gradient_Viscoelastics
2227 *
2228 * INPUT
2229 *
2230 * Pos_Array - positions of the masses
2231 *
2232 * OUTPUT
2233 *
2234 * calculated gradients of the potential fields are added to 'Gradient'.
2235 *
2236 * RETURNS
2237 *
2238 * AUTHOR
2239 *
2240 * Christoph Hormann
2241 *
2242 * DESCRIPTION
2243 *
2244 * viscoelastic elements are treated as elastic connections with
2245 * 'Stiffness0'
2246 *
2247 * CHANGES
2248 *
2249 * --- Feb 2005 : Creation
2250 *
2251 ******************************************************************************/
2252
Mechsim_Gradient_Viscoelastics(VECTOR * Gradient,VECTOR * Pos_Array)2253 void Mechsim_Gradient_Viscoelastics(VECTOR *Gradient, VECTOR *Pos_Array)
2254 {
2255 int i;
2256 DBL lAxis;
2257 VECTOR Axis;
2258 VECTOR pot_force;
2259
2260 for (i=0; i<MechsimOptions.Data.ve_count; i++)
2261 {
2262 VSub(Axis, Pos_Array[MechsimOptions.Data.Viscoelastics[i].Idx1],
2263 Pos_Array[MechsimOptions.Data.Viscoelastics[i].Idx2] );
2264 VLength(lAxis, Axis);
2265
2266 if (lAxis>0) // otherwise we can't conclude the direction
2267 {
2268 // --- potential force ---
2269 VScale(pot_force, Axis, -((lAxis-MechsimOptions.Data.Viscoelastics[i].Length)/lAxis)*MechsimOptions.Data.Viscoelastics[i].Stiffness0);
2270
2271 VAddEq(Gradient[MechsimOptions.Data.Viscoelastics[i].Idx1], pot_force);
2272 VSubEq(Gradient[MechsimOptions.Data.Viscoelastics[i].Idx2], pot_force);
2273
2274 }
2275 }
2276 }
2277
2278
2279 /*****************************************************************************
2280 *
2281 * FUNCTION
2282 *
2283 * Mechsim_Force_Viscoelastics
2284 *
2285 * INPUT
2286 *
2287 * State_Array - state of the masses
2288 *
2289 * OUTPUT
2290 *
2291 * calculated accelerations are added to 'Accel'.
2292 *
2293 * RETURNS
2294 *
2295 * AUTHOR
2296 *
2297 * Christoph Hormann
2298 *
2299 * DESCRIPTION
2300 *
2301 * Calculates the forces resulting from viscoelastic elements
2302 *
2303 * CHANGES
2304 *
2305 * --- Feb 2005 : Creation
2306 *
2307 ******************************************************************************/
2308
Mechsim_Force_Viscoelastics(VECTOR * Accel,VECTOR * State_Array)2309 void Mechsim_Force_Viscoelastics(VECTOR *Accel, VECTOR *State_Array)
2310 {
2311 int i, j;
2312 DBL lAxis, Delta, Force_Val;
2313 VECTOR Axis;
2314 VECTOR force, pot_force1, pot_force2;
2315 DBL damp_fact;
2316
2317 for (i=0; i<MechsimOptions.Data.ve_count; i++)
2318 {
2319 VSub(Axis, State_Array[MechsimOptions.Data.Viscoelastics[i].Idx1],
2320 State_Array[MechsimOptions.Data.Viscoelastics[i].Idx2] );
2321 VLength(lAxis, Axis);
2322
2323 if (lAxis>0) // otherwise we can't conclude the direction
2324 {
2325 Delta = lAxis-MechsimOptions.Data.Viscoelastics[i].Length;
2326
2327 // --- plain elastic force ---
2328 Force_Val = Delta*MechsimOptions.Data.Viscoelastics[i].Stiffness0;
2329
2330 // --- element forces ---
2331 for (j=0; j<MechsimOptions.Data.Viscoelastics[i].element_count; j++)
2332 {
2333 Force_Val += (Delta - MechsimOptions.Data.Viscoelastics[i].Element[j].Y)*MechsimOptions.Data.Viscoelastics[i].Element[j].Stiffness;
2334 }
2335
2336 VScale(force, Axis, -Force_Val/lAxis);
2337
2338 VScale(pot_force1, force, 1.0/MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[i].Idx1].Mass);
2339 VScale(pot_force2, force, -1.0/MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[i].Idx2].Mass);
2340
2341 VAddEq(Accel[MechsimOptions.Data.Viscoelastics[i].Idx1], pot_force1);
2342 VAddEq(Accel[MechsimOptions.Data.Viscoelastics[i].Idx2], pot_force2);
2343
2344 }
2345 }
2346 }
2347
2348
2349 /*****************************************************************************
2350 *
2351 * FUNCTION
2352 *
2353 * Mechsim_Viscoelastic_Evaluate
2354 *
2355 * INPUT
2356 *
2357 * Idx - number of viscoelastic connection to evaluate
2358 *
2359 * OUTPUT
2360 *
2361 * RETURNS
2362 *
2363 * The force of the connection
2364 *
2365 * AUTHOR
2366 *
2367 * Christoph Hormann
2368 *
2369 * DESCRIPTION
2370 *
2371 * Calculates the force resulting from a viscoelastic connection.
2372 *
2373 * This is supposed to be called after the simulation is finished.
2374 *
2375 * CHANGES
2376 *
2377 * --- Mar 2005 : Creation
2378 *
2379 ******************************************************************************/
2380
Mechsim_Viscoelastic_Evaluate(int Idx)2381 DBL Mechsim_Viscoelastic_Evaluate(int Idx)
2382 {
2383 int j;
2384 DBL lAxis, xForce, Delta;
2385 VECTOR Axis;
2386 DBL damp_fact;
2387
2388 VSub(Axis, MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[Idx].Idx1].Position,
2389 MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[Idx].Idx2].Position );
2390 VLength(lAxis, Axis);
2391
2392 if (lAxis>0) // otherwise we can't conclude the direction
2393 {
2394 Delta = lAxis-MechsimOptions.Data.Viscoelastics[Idx].Length;
2395
2396 // --- plain elastic force ---
2397 xForce = Delta*MechsimOptions.Data.Viscoelastics[Idx].Stiffness0;
2398
2399 // --- element forces ---
2400 for (j=0; j<MechsimOptions.Data.Viscoelastics[Idx].element_count; j++)
2401 {
2402 xForce += (Delta - MechsimOptions.Data.Viscoelastics[Idx].Element[j].Y)*MechsimOptions.Data.Viscoelastics[Idx].Element[j].Stiffness;
2403 }
2404
2405 }
2406 else xForce = 0.0;
2407
2408 return xForce;
2409 }
2410
2411
2412 /*****************************************************************************
2413 *
2414 * FUNCTION
2415 *
2416 * Mechsim_Integrate_Viscoelastics
2417 *
2418 * INPUT
2419 *
2420 * State_Array - state of the masses
2421 *
2422 * OUTPUT
2423 *
2424 * -
2425 *
2426 * RETURNS
2427 *
2428 * AUTHOR
2429 *
2430 * Christoph Hormann
2431 *
2432 * DESCRIPTION
2433 *
2434 * Calculates the inner variables of all viscoelastic elements.
2435 * To be called once every simulation step.
2436 *
2437 * The inner variables (see docs) follow the following ODE:
2438 *
2439 * dy/dt = Stiffness/Damping * ( Delta - y )
2440 *
2441 * with Delta = L-L0 (Length extension)
2442 *
2443 * This function integrates this equation. It uses first order
2444 * explicit method (Euler) with adjustable time stepping (via Accuracy)
2445 * but with linear elements this is probably non-critical.
2446 *
2447 * CHANGES
2448 *
2449 * --- Feb 2005 : Creation
2450 *
2451 ******************************************************************************/
2452
Mechsim_Integrate_Viscoelastics(VECTOR * State_Array)2453 void Mechsim_Integrate_Viscoelastics(VECTOR *State_Array)
2454 {
2455 int i, j, k;
2456 DBL lAxis, Delta, Alpha, H;
2457 VECTOR Axis;
2458
2459 for (i=0; i<MechsimOptions.Data.ve_count; i++)
2460 {
2461 VSub(Axis, State_Array[MechsimOptions.Data.Viscoelastics[i].Idx1],
2462 State_Array[MechsimOptions.Data.Viscoelastics[i].Idx2] );
2463 VLength(lAxis, Axis);
2464
2465 if (lAxis>0) // otherwise we can't conclude the direction
2466 {
2467 VInverseScaleEq(Axis, lAxis); // Normalize direction vector
2468
2469 Delta = lAxis-MechsimOptions.Data.Viscoelastics[i].Length;
2470 H = MechsimOptions.Time_Step/(DBL)MechsimOptions.Data.Viscoelastics[i].Accuracy;
2471
2472 for (j=0; j<MechsimOptions.Data.Viscoelastics[i].element_count; j++)
2473 {
2474 Alpha = MechsimOptions.Data.Viscoelastics[i].Element[j].Stiffness/MechsimOptions.Data.Viscoelastics[i].Element[j].Damping;
2475
2476 for (k=0; k<MechsimOptions.Data.Viscoelastics[i].Accuracy; k++)
2477 MechsimOptions.Data.Viscoelastics[i].Element[j].Y += H * Alpha * ( Delta - MechsimOptions.Data.Viscoelastics[i].Element[j].Y );
2478
2479 //Debug_Info("Viscoelastics %g %g\n", Delta, MechsimOptions.Data.Viscoelastics[i].Element[j].Y);
2480 #if(MECHSIM_DEBUG == 1)
2481 // Debug_Info("Viscoelastics %g %g\n", Delta, MechsimOptions.Data.Viscoelastics[i].Element[j].Y);
2482 #endif
2483
2484 }
2485 }
2486 }
2487 }
2488
2489
2490 /*****************************************************************************
2491 *
2492 * FUNCTION
2493 *
2494 * Mechsim_Gradient_Interactions
2495 *
2496 * INPUT
2497 *
2498 * Pos_Array - positions of the masses
2499 *
2500 * OUTPUT
2501 *
2502 * calculated accelerations are added to 'Accel'.
2503 *
2504 * RETURNS
2505 *
2506 * AUTHOR
2507 *
2508 * Christoph Hormann
2509 *
2510 * DESCRIPTION
2511 *
2512 * Gradient descent version for the interactions
2513 *
2514 * CHANGES
2515 *
2516 * --- Nov 2002 : Creation
2517 * Mar 2005 : modified for two separate function evaluations
2518 *
2519 ******************************************************************************/
2520
Mechsim_Gradient_Interactions(VECTOR * Accel,VECTOR * Pos_Array)2521 void Mechsim_Gradient_Interactions(VECTOR *Accel, VECTOR *Pos_Array)
2522 {
2523 int i, j, k;
2524 DBL Fn_Val1, Fn_Val2, lAxis;
2525 VECTOR Axis;
2526
2527 for (i=0; i<MechsimOptions.Data.mass_count; i++)
2528 {
2529 for (j=0; j<i; j++)
2530 /*
2531 This double loop accesses each combination of
2532 two different masses exactly once
2533 */
2534 {
2535 VSub(Axis, Pos_Array[i], Pos_Array[j] );
2536 VLength(lAxis, Axis);
2537
2538 if (lAxis>0) // otherwise we can't conclude the direction
2539 {
2540 VInverseScaleEq(Axis, lAxis); // Normalize direction vector
2541
2542 for (k=0; k<MechsimOptions.Interactions.count; k++)
2543 {
2544 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Interactions.Function[k]));
2545
2546 // evaluate function for first mass
2547 POVFPU_SetLocal(X, Pos_Array[i][X]);
2548 POVFPU_SetLocal(Y, Pos_Array[i][Y]);
2549 POVFPU_SetLocal(Z, Pos_Array[i][Z]);
2550 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
2551 // additional mass 1 parameter
2552 if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[i].Mass);
2553 // additional mass 2 parameter
2554 if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[j].Mass);
2555
2556 Fn_Val1 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));
2557
2558 // evaluate function for second mass
2559 POVFPU_SetLocal(X, Pos_Array[j][X]);
2560 POVFPU_SetLocal(Y, Pos_Array[j][Y]);
2561 POVFPU_SetLocal(Z, Pos_Array[j][Z]);
2562 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
2563 // additional mass 1 parameter
2564 if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[j].Mass);
2565 // additional mass 2 parameter
2566 if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[i].Mass);
2567
2568 Fn_Val2 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));
2569
2570 if (fabs(Fn_Val1) > MECHSIM_EPSILON)
2571 {
2572 /* apply force to first mass */
2573 VAddScaledEq(Accel[i], -Fn_Val1, Axis);
2574 }
2575 if (fabs(Fn_Val2) > MECHSIM_EPSILON)
2576 {
2577 /* apply force to second mass */
2578 VAddScaledEq(Accel[j], Fn_Val2, Axis);
2579 }
2580 }
2581 }
2582 }
2583 }
2584
2585 }
2586
2587 /*****************************************************************************
2588 *
2589 * FUNCTION
2590 *
2591 * Mechsim_Force_Interactions
2592 *
2593 * INPUT
2594 *
2595 * Pos_Array - positions of the masses
2596 *
2597 * OUTPUT
2598 *
2599 * calculated accelerations are added to 'Accel'.
2600 *
2601 * RETURNS
2602 *
2603 * AUTHOR
2604 *
2605 * Christoph Hormann
2606 *
2607 * DESCRIPTION
2608 *
2609 * Calculates the function defined interactions between the masses
2610 *
2611 * Interaction functions can have 4 parameters: the three spartial
2612 * coordinates and the distance between the masses.
2613 *
2614 * CHANGES
2615 *
2616 * --- Nov 2002 : Creation
2617 * Mar 2005 : modified for two separate function evaluations
2618 *
2619 ******************************************************************************/
2620
Mechsim_Force_Interactions(VECTOR * Accel,VECTOR * Pos_Array)2621 void Mechsim_Force_Interactions(VECTOR *Accel, VECTOR *Pos_Array)
2622 {
2623 int i, j, k;
2624 DBL Fn_Val1, Fn_Val2, lAxis;
2625 VECTOR Axis;
2626
2627 for (i=0; i<MechsimOptions.Data.mass_count; i++)
2628 {
2629 for (j=0; j<i; j++)
2630 /*
2631 This double loop accesses each combination of
2632 two different masses exactly once
2633 */
2634 {
2635 VSub(Axis, Pos_Array[i], Pos_Array[j] );
2636 VLength(lAxis, Axis);
2637
2638 if (lAxis>0) // otherwise we can't conclude the direction
2639 {
2640 VInverseScaleEq(Axis, lAxis); // Normalize direction vector
2641
2642 for (k=0; k<MechsimOptions.Interactions.count; k++)
2643 {
2644 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Interactions.Function[k]));
2645
2646 // evaluate function for first mass
2647 POVFPU_SetLocal(X, Pos_Array[i][X]);
2648 POVFPU_SetLocal(Y, Pos_Array[i][Y]);
2649 POVFPU_SetLocal(Z, Pos_Array[i][Z]);
2650 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
2651 // additional mass 1 parameter
2652 if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[i].Mass);
2653 // additional mass 2 parameter
2654 if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[j].Mass);
2655
2656 Fn_Val1 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));
2657
2658 // evaluate function for second mass
2659 POVFPU_SetLocal(X, Pos_Array[j][X]);
2660 POVFPU_SetLocal(Y, Pos_Array[j][Y]);
2661 POVFPU_SetLocal(Z, Pos_Array[j][Z]);
2662 if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
2663 // additional mass 1 parameter
2664 if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[j].Mass);
2665 // additional mass 2 parameter
2666 if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[i].Mass);
2667
2668 Fn_Val2 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));
2669
2670 if (fabs(Fn_Val1) > MECHSIM_EPSILON)
2671 {
2672 /* apply force to first mass */
2673 VAddScaledEq(Accel[i], -Fn_Val1/MechsimOptions.Data.Masses[i].Mass, Axis);
2674 }
2675 if (fabs(Fn_Val2) > MECHSIM_EPSILON)
2676 {
2677 /* apply force to second mass */
2678 VAddScaledEq(Accel[j], Fn_Val2/MechsimOptions.Data.Masses[j].Mass, Axis);
2679 }
2680 }
2681 }
2682 }
2683 }
2684
2685 }
2686
2687 /*****************************************************************************
2688 *
2689 * FUNCTION
2690 *
2691 * Mechsim_Gradient_Fields
2692 *
2693 * INPUT
2694 *
2695 * Index - index of the mass
2696 * Accel - accelerations of the masses
2697 * Pos_Array - positions of the masses
2698 *
2699 * OUTPUT
2700 *
2701 * calculated acceleration is added to 'Accel'.
2702 *
2703 * RETURNS
2704 *
2705 * AUTHOR
2706 *
2707 * Christoph Hormann
2708 *
2709 * DESCRIPTION
2710 *
2711 * Gradient descent version for the field forces
2712 *
2713 * CHANGES
2714 *
2715 * --- Aug 2002 : Creation
2716 * Nov 2002 : Added custom force fields
2717 *
2718 ******************************************************************************/
2719
Mechsim_Gradient_Fields(int Index,VECTOR Accel,VECTOR * Pos_Array)2720 void Mechsim_Gradient_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array)
2721 {
2722 int k, param;
2723
2724 VAddEq( Accel, MechsimOptions.gravity);
2725
2726 for (k=0; k<MechsimOptions.Fields.count; k++)
2727 {
2728 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Fields.Function[k]));
2729
2730 POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
2731 POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
2732 POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);
2733
2734 (void)POVFPU_Run(*(MechsimOptions.Fields.Function[k]));
2735
2736 if (f->return_size >= 3)
2737 for(param = 0; param < 3; param++)
2738 Accel[param] += POVFPU_GetLocal(param);
2739 }
2740 }
2741
2742 /*****************************************************************************
2743 *
2744 * FUNCTION
2745 *
2746 * Mechsim_Force_Fields
2747 *
2748 * INPUT
2749 *
2750 * Index - index of the mass
2751 * Accel - accelerations of the masses
2752 * Pos_Array - positions of the masses
2753 *
2754 * OUTPUT
2755 *
2756 * calculated acceleration is added to 'Accel'.
2757 *
2758 * RETURNS
2759 *
2760 * AUTHOR
2761 *
2762 * Christoph Hormann
2763 *
2764 * DESCRIPTION
2765 *
2766 * Calculates the field forces on a point mass
2767 *
2768 * CHANGES
2769 *
2770 * --- Aug 2002 : Creation
2771 * Nov 2002 : Added custom force fields
2772 *
2773 ******************************************************************************/
2774
Mechsim_Force_Fields(int Index,VECTOR Accel,VECTOR * Pos_Array)2775 void Mechsim_Force_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array)
2776 {
2777 int k, param;
2778
2779 VAddEq( Accel, MechsimOptions.gravity);
2780
2781 for (k=0; k<MechsimOptions.Fields.count; k++)
2782 {
2783 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Fields.Function[k]));
2784
2785 POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
2786 POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
2787 POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);
2788
2789 (void)POVFPU_Run(*(MechsimOptions.Fields.Function[k]));
2790
2791 if (f->return_size >= 3)
2792 for(param = 0; param < 3; param++)
2793 Accel[param] += POVFPU_GetLocal(param) / MechsimOptions.Data.Masses[Index].Mass;
2794 }
2795 }
2796
2797 /*****************************************************************************
2798 *
2799 * FUNCTION
2800 *
2801 * Mechsim_Gradient_Forces
2802 *
2803 * INPUT
2804 *
2805 * Index - index of the mass
2806 * Accel - accelerations of the masses
2807 * Pos_Array - positions of the masses
2808 *
2809 * OUTPUT
2810 *
2811 * calculated acceleration is added to 'Accel'.
2812 *
2813 * RETURNS
2814 *
2815 * AUTHOR
2816 *
2817 * Christoph Hormann
2818 *
2819 * DESCRIPTION
2820 *
2821 * Gradient descent version for the custom forces function
2822 *
2823 * CHANGES
2824 *
2825 * --- Apr 2005 : Creation
2826 *
2827 ******************************************************************************/
2828
Mechsim_Gradient_Forces(int Index,VECTOR Accel,VECTOR * Pos_Array)2829 void Mechsim_Gradient_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array)
2830 {
2831 int param;
2832
2833 if (MechsimOptions.Forces.count>MechsimOptions.Data.Masses[Index].Force)
2834 {
2835 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));
2836
2837 POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
2838 POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
2839 POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);
2840
2841 (void)POVFPU_Run(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));
2842
2843 if (f->return_size >= 3)
2844 for(param = 0; param < 3; param++)
2845 Accel[param] += POVFPU_GetLocal(param);
2846 }
2847
2848 }
2849
2850 /*****************************************************************************
2851 *
2852 * FUNCTION
2853 *
2854 * Mechsim_Force_Forces
2855 *
2856 * INPUT
2857 *
2858 * Index - index of the mass
2859 * Accel - accelerations of the masses
2860 * Pos_Array - positions of the masses
2861 *
2862 * OUTPUT
2863 *
2864 * calculated acceleration is added to 'Accel'.
2865 *
2866 * RETURNS
2867 *
2868 * AUTHOR
2869 *
2870 * Christoph Hormann
2871 *
2872 * DESCRIPTION
2873 *
2874 * Calculates the custom forces on a point mass
2875 *
2876 * CHANGES
2877 *
2878 * --- Apr 2005 : Creation
2879 *
2880 ******************************************************************************/
2881
Mechsim_Force_Forces(int Index,VECTOR Accel,VECTOR * Pos_Array)2882 void Mechsim_Force_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array)
2883 {
2884 int param;
2885
2886 if (MechsimOptions.Forces.count>MechsimOptions.Data.Masses[Index].Force)
2887 {
2888 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));
2889
2890 POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
2891 POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
2892 POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);
2893
2894 (void)POVFPU_Run(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));
2895
2896 if (f->return_size >= 3)
2897 for(param = 0; param < 3; param++)
2898 Accel[param] += POVFPU_GetLocal(param) / MechsimOptions.Data.Masses[Index].Mass;
2899
2900 }
2901
2902 }
2903
2904 /*****************************************************************************
2905 *
2906 * FUNCTION
2907 *
2908 * Mechsim_Gradients
2909 *
2910 * INPUT
2911 *
2912 * Pos_Array - state of the masses
2913 * Vel_Array - *** Different usage here ***
2914 * stores the last position of the mass
2915 * Time - current time index for the environment function
2916 *
2917 * OUTPUT
2918 *
2919 * calculated acceleration is returned via 'Accel'.
2920 *
2921 * RETURNS
2922 *
2923 * AUTHOR
2924 *
2925 * Christoph Hormann
2926 *
2927 * DESCRIPTION
2928 *
2929 * Wrapper for calculating the gradients of the potential fields
2930 *
2931 * CHANGES
2932 *
2933 * --- Sep 2002 : Creation
2934 * Feb 2005 : added support for global fixed
2935 * Apr 2005 : added custom Forces
2936 *
2937 ******************************************************************************/
2938
Mechsim_Gradients(VECTOR * Gradient,VECTOR * Pos_Array,VECTOR * Vel_Array,DBL Time,MECHSIM_BOUNDING_DATA * Bounding_Data)2939 void Mechsim_Gradients(VECTOR *Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
2940 {
2941 int i, j;
2942 bool Fixed;
2943
2944 if (MechsimOptions.Fixed[0] || MechsimOptions.Fixed[1] || MechsimOptions.Fixed[2])
2945 Fixed = true;
2946 else
2947 Fixed = false;
2948
2949 /* first set all Gradients to 0.0 */
2950 for (i=0; i<MechsimOptions.Data.mass_count; i++)
2951 Make_Vector(Gradient[i], 0.0, 0.0, 0.0);
2952
2953 /* calculate gradients that apply to several masses */
2954 if ((MechsimOptions.Calc_Collision != MECHSIM_COLLISION_NONE) ||
2955 (MechsimOptions.Calc_Collision_Connections != MECHSIM_COLLISION_NONE) ||
2956 (MechsimOptions.Calc_Collision_Faces != MECHSIM_COLLISION_NONE))
2957 Mechsim_Collide_Dynamic(Gradient, Pos_Array, NULL, Bounding_Data);
2958
2959 if (MechsimOptions.Interactions.count>0)
2960 Mechsim_Gradient_Interactions(Gradient, Pos_Array);
2961
2962 if (MechsimOptions.Data.connect_count>0)
2963 Mechsim_Gradient_Connect(Gradient, Pos_Array);
2964
2965 if (MechsimOptions.Data.ve_count>0)
2966 Mechsim_Gradient_Viscoelastics(Gradient, Pos_Array);
2967
2968 for (i=0; i<MechsimOptions.Data.mass_count; i++)
2969 {
2970 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
2971 {
2972 /* calculate gradients that only apply to this mass */
2973
2974 for (j=0; j<MechsimOptions.Env_count; j++) // Environments
2975 {
2976 if (MechsimOptions.Environment[j].Function != NULL || MechsimOptions.Environment[j].Object != NULL)
2977 Mechsim_Gradient_Collide_Environment(i, Gradient[i], Pos_Array, Vel_Array, Time,
2978 &MechsimOptions.Environment[j]);
2979 }
2980
2981 Mechsim_Gradient_Fields(i, Gradient[i], Pos_Array);
2982
2983 if (MechsimOptions.Forces.count>0)
2984 Mechsim_Gradient_Forces(i, Gradient[i], Pos_Array);
2985
2986 /* handle global fixed */
2987 if (Fixed)
2988 {
2989 if (MechsimOptions.Fixed[0])
2990 {
2991 Gradient[i][X] = 0.0;
2992 }
2993 if (MechsimOptions.Fixed[1])
2994 {
2995 Gradient[i][Y] = 0.0;
2996 }
2997 if (MechsimOptions.Fixed[2])
2998 {
2999 Gradient[i][Z] = 0.0;
3000 }
3001 }
3002
3003 }
3004 }
3005 }
3006
3007 /*****************************************************************************
3008 *
3009 * FUNCTION
3010 *
3011 * Mechsim_Forces
3012 *
3013 * INPUT
3014 *
3015 * State_Array - state of the masses (position and velocity)
3016 * Time - current time index (for the environment function)
3017 * Bounding_Data - bounding information for collisions
3018 *
3019 * OUTPUT
3020 *
3021 * calculated Force Vector is returned via 'Forces'.
3022 *
3023 * RETURNS
3024 *
3025 * AUTHOR
3026 *
3027 * Christoph Hormann
3028 *
3029 * DESCRIPTION
3030 *
3031 * Wrapper for calculating all forces on all masses
3032 *
3033 * ------------------------------------------------------------------
3034 *
3035 * The arrays are used as follows:
3036 *
3037 * State_Array - describes the current state of the system
3038 * (at time 'Time'):
3039 *
3040 *
3041 * 0 mass_count mass_count*2
3042 * -------------------------------
3043 * | | |
3044 * | Positions | Velocities |
3045 * | | |
3046 * -------------------------------
3047 *
3048 * Forces - contains the 'right side' of the ODE,
3049 * converted to first order the following way:
3050 *
3051 * x'' = f(x, x', t)
3052 *
3053 * --> X' = F(X, t)
3054 *
3055 * with X = <x, x'> and X' = <x', x''>
3056 *
3057 * ==> F(X, t) = <x', f(x, x', t)>
3058 *
3059 *
3060 *
3061 * 0 mass_count mass_count*2
3062 * -------------------------------
3063 * | | |
3064 * | Velocities | Accelerations |
3065 * | | |
3066 * -------------------------------
3067 *
3068 * ------------------------------------------------------------------
3069 *
3070 * The local identifiers 'Vel_Array' and 'Accel' are used to directly
3071 * access the second part of these arrays.
3072 *
3073 *
3074 *
3075 *
3076 *
3077 * CHANGES
3078 *
3079 * --- Aug 2002 : Creation
3080 * Jan 2005 : Modified for new data structures
3081 * Feb 2005 : added support for global fixed
3082 * Apr 2005 : added custom Forces
3083 *
3084 ******************************************************************************/
3085
Mechsim_Forces(VECTOR * Forces,VECTOR * State_Array,DBL Time,MECHSIM_BOUNDING_DATA * Bounding_Data)3086 void Mechsim_Forces(VECTOR *Forces, VECTOR *State_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
3087 {
3088 int i, j;
3089 bool Fixed;
3090 VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
3091 VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];
3092
3093 if (MechsimOptions.Fixed[0] || MechsimOptions.Fixed[1] || MechsimOptions.Fixed[2])
3094 Fixed = true;
3095 else
3096 Fixed = false;
3097
3098 /* first set all Accelerations to 0.0 and copy Velocities to Forces Array */
3099 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3100 {
3101 Assign_Vector(Forces[i], Vel_Array[i]);
3102 Make_Vector(Accel[i], 0.0, 0.0, 0.0);
3103 }
3104
3105 /* calculate forces that apply to several masses */
3106 if ((MechsimOptions.Calc_Collision != MECHSIM_COLLISION_NONE) ||
3107 (MechsimOptions.Calc_Collision_Connections != MECHSIM_COLLISION_NONE) ||
3108 (MechsimOptions.Calc_Collision_Faces != MECHSIM_COLLISION_NONE))
3109 Mechsim_Collide_Dynamic(Accel, State_Array, Vel_Array, Bounding_Data);
3110
3111 if (MechsimOptions.Interactions.count>0)
3112 Mechsim_Force_Interactions(Accel, State_Array);
3113
3114 if (MechsimOptions.Data.connect_count>0)
3115 Mechsim_Force_Connect(Accel, State_Array, Vel_Array);
3116
3117 if (MechsimOptions.Data.ve_count>0)
3118 Mechsim_Force_Viscoelastics(Accel, State_Array);
3119
3120 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3121 {
3122 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
3123 {
3124 /* calculate forces that only apply to this mass */
3125 for (j=0; j<MechsimOptions.Env_count; j++) // Environments
3126 {
3127 if (MechsimOptions.Environment[j].Method==2)
3128 {
3129 if (MechsimOptions.Environment[j].Function != NULL || MechsimOptions.Environment[j].Object != NULL)
3130 Mechsim_Impact_Collide_Environment(i, State_Array, Vel_Array, Time,
3131 &MechsimOptions.Environment[j]);
3132 }
3133 else
3134 {
3135 if (MechsimOptions.Environment[j].Function != NULL || MechsimOptions.Environment[j].Object != NULL)
3136 Mechsim_Force_Collide_Environment(i, Accel[i], State_Array, Vel_Array, Time,
3137 &MechsimOptions.Environment[j]);
3138 }
3139 }
3140
3141 Mechsim_Force_Fields(i, Accel[i], State_Array);
3142
3143 if (MechsimOptions.Data.Masses[i].Force>=0)
3144 Mechsim_Force_Forces(i, Accel[i], State_Array);
3145
3146 /* handle global fixed */
3147 if (Fixed)
3148 {
3149 if (MechsimOptions.Fixed[0])
3150 {
3151 Accel[i][X] = 0.0;
3152 Vel_Array[i][X] = 0.0;
3153 }
3154 if (MechsimOptions.Fixed[1])
3155 {
3156 Accel[i][Y] = 0.0;
3157 Vel_Array[i][Y] = 0.0;
3158 }
3159 if (MechsimOptions.Fixed[2])
3160 {
3161 Accel[i][Z] = 0.0;
3162 Vel_Array[i][Z] = 0.0;
3163 }
3164 }
3165 #if(MECHSIM_DEBUG == 1)
3166 // if ((Steps==0) && (i==0)) Debug_Info("\nAcc=<%g, %g, %g>\n", Accel[0][X],Accel[0][Y],Accel[0][Z] );
3167 #endif
3168 }
3169 else
3170 {
3171 Make_Vector(Accel[i], 0.0, 0.0, 0.0);
3172 Make_Vector(Vel_Array[i], 0.0, 0.0, 0.0);
3173 }
3174 }
3175
3176 }
3177
3178 /*****************************************************************************
3179 *
3180 * FUNCTION
3181 *
3182 * Mechsim_Check_Col
3183 *
3184 * INPUT
3185 *
3186 * Pos - new position
3187 * Old_Pos - old position to restore when mass is inside environment shapes
3188 *
3189 * OUTPUT
3190 *
3191 * RETURNS
3192 *
3193 * AUTHOR
3194 *
3195 * Christoph Hormann
3196 *
3197 * DESCRIPTION
3198 *
3199 * Penetration prevention for object intersection based environments
3200 *
3201 * CHANGES
3202 *
3203 * --- Oct 2002 : Creation
3204 *
3205 ******************************************************************************/
3206
Mechsim_Check_Col(VECTOR Pos,VECTOR Old_Pos)3207 void Mechsim_Check_Col(VECTOR Pos, VECTOR Old_Pos)
3208 {
3209 MECHSIM_ENVIRONMENT *Env;
3210
3211 if (MechsimOptions.Environment != NULL) // Environments
3212 {
3213 Env = MechsimOptions.Environment;
3214
3215 while (Env != NULL)
3216 {
3217 if (Env->Function == NULL && Env->Object != NULL) // only for object based environments
3218 if (Inside_Object(Pos, Env->Object))
3219 {
3220 Assign_Vector(Pos, Old_Pos);
3221 Increase_Counter(stats[MechSim_Environment_Penetrations]);
3222 }
3223
3224 Env = Env->Next;
3225 }
3226 }
3227
3228 }
3229
3230 /*****************************************************************************
3231 *
3232 * FUNCTION
3233 *
3234 * Mechsim_Check_Instability
3235 *
3236 * INPUT
3237 *
3238 * Pos - position of mass
3239 * Vel - velocity of mass
3240 * Accel - acceleration of mass
3241 *
3242 * OUTPUT
3243 *
3244 * RETURNS
3245 *
3246 * AUTHOR
3247 *
3248 * Christoph Hormann
3249 *
3250 * DESCRIPTION
3251 *
3252 * Tries to do a rough estimation if the simulation grows instable and
3253 * drifts into unrealistic stages. Extreme instabilities might cause
3254 * program crashes if not detected.
3255 *
3256 * CHANGES
3257 *
3258 * --- Dec 2002 : Creation
3259 * Mar 2005 : increased threshold
3260 *
3261 ******************************************************************************/
3262
Mechsim_Check_Instability(VECTOR Pos,VECTOR Vel,VECTOR Accel)3263 void Mechsim_Check_Instability(VECTOR Pos, VECTOR Vel, VECTOR Accel)
3264 {
3265 DBL lAcc;
3266
3267 VLength(lAcc, Accel);
3268
3269 /*
3270 simply test if the acceleration exceeds a certain value.
3271 the exact value is probably not critical since an acceleration
3272 just a bit lower will be likely to cause an even higher value
3273 some steps later.
3274 */
3275
3276 if (lAcc > 1.0e200)
3277 //if (lAcc > 1.0e12)
3278 Error("Instability detected during simulation. Using smaller\ntime steps or changing the system parameters might help.\n");
3279
3280 }
3281
3282 /*****************************************************************************
3283 *
3284 * FUNCTION
3285 *
3286 * Mechsim_Euler_Step
3287 *
3288 * INPUT
3289 *
3290 * Forces - To store the evaluations of the right side of the equation
3291 * State_Array - new system state (does not need to be initialized)
3292 * State_Old - old system state
3293 * Time - current time value
3294 * Bounding_Data - bounding information for collisions
3295 *
3296 * OUTPUT
3297 *
3298 * RETURNS
3299 *
3300 * new time = old time + fixed time step
3301 *
3302 * AUTHOR
3303 *
3304 * Christoph Hormann
3305 *
3306 * DESCRIPTION
3307 *
3308 * Runs a single step for numerical integration with Euler method
3309 *
3310 * ===============================================
3311 * Euler-Method:
3312 * ===============================================
3313 *
3314 * Formula:
3315 *
3316 * y_i+1 = y_i + h*f(y_i, t)
3317 *
3318 * Note: y_i/y_i+1 represents both positions and velocities
3319 *
3320 * For all time steps the forces are calculated,
3321 * results ('f(y_i, t)') are stored in 'Forces' and then
3322 * the are applied to all mass positions.
3323 *
3324 * CHANGES
3325 *
3326 * --- Apr 2003 : Moved out of Mechsim_Simulate()
3327 * Jan 2005 : Extracted function for single time step
3328 *
3329 ******************************************************************************/
3330
Mechsim_Euler_Step(VECTOR * Forces,VECTOR * State_Array,VECTOR * State_Old,DBL Time,MECHSIM_BOUNDING_DATA * Bounding_Data)3331 DBL Mechsim_Euler_Step(VECTOR *Forces, VECTOR *State_Array, VECTOR *State_Old, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
3332 {
3333 int i, j;
3334
3335 /* calculate all forces at y_i */
3336 Mechsim_Forces(Forces, State_Old, Time, Bounding_Data);
3337 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3338
3339 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3340 {
3341 j = i + MechsimOptions.Data.mass_count;
3342
3343 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
3344 {
3345 /* y_new = y_old + h*f(y_old) */
3346 State_Array[i][X] = State_Old[i][X] + MechsimOptions.Time_Step*Forces[i][X];
3347 State_Array[i][Y] = State_Old[i][Y] + MechsimOptions.Time_Step*Forces[i][Y];
3348 State_Array[i][Z] = State_Old[i][Z] + MechsimOptions.Time_Step*Forces[i][Z];
3349
3350 State_Array[j][X] = State_Old[j][X] + MechsimOptions.Time_Step*Forces[j][X];
3351 State_Array[j][Y] = State_Old[j][Y] + MechsimOptions.Time_Step*Forces[j][Y];
3352 State_Array[j][Z] = State_Old[j][Z] + MechsimOptions.Time_Step*Forces[j][Z];
3353
3354 Mechsim_Check_Instability(State_Array[i], State_Array[j], Forces[j]);
3355 }
3356 else
3357 {
3358 Assign_Vector(State_Array[i], State_Old[i]);
3359 Make_Vector(State_Array[j], 0.0, 0.0, 0.0);
3360 }
3361 Do_Cooperate(1);
3362 }
3363
3364 return Time+MechsimOptions.Time_Step;
3365 }
3366
3367
3368 /*****************************************************************************
3369 *
3370 * FUNCTION
3371 *
3372 * Mechsim_Heun_Step
3373 *
3374 * INPUT
3375 *
3376 * Forces - To store the evaluations of the right side of the equation
3377 * State_Array - new system state (does not need to be initialized)
3378 * State_Old - old system state
3379 * k1 - in between evaluation
3380 * Time - current time value
3381 * Bounding_Data - bounding information for collisions
3382 *
3383 * OUTPUT
3384 *
3385 * RETURNS
3386 *
3387 * new time = old time + fixed time step
3388 *
3389 * AUTHOR
3390 *
3391 * Christoph Hormann
3392 *
3393 * DESCRIPTION
3394 *
3395 * Runs a single step for numerical integration with Heun method
3396 *
3397 * ===============================================
3398 * Heun-Method:
3399 * ===============================================
3400 *
3401 * Formula:
3402 *
3403 * y_i+1 = y_i + (h/2)*f(k_1 + k_2)
3404 *
3405 * with:
3406 *
3407 * k_1 = f(y_i, t)
3408 * k_2 = f(y_i + h*k_1, t + h)
3409 *
3410 * Note: y_i/y_i+1 represents both positions and velocities
3411 *
3412 * k_1 is required to be stored for all masses while
3413 * k_2 is calculated in the loop for 'y_i+1'. The in between
3414 * state (y2 = y_i + h*k_1) is stored in Pos2/Vel2.
3415 *
3416 * CHANGES
3417 *
3418 * --- Apr 2003 : Moved out of Mechsim_Simulate()
3419 * Jan 2005 : Extracted function for single time step
3420 *
3421 ******************************************************************************/
3422
Mechsim_Heun_Step(VECTOR * Forces,VECTOR * State_Array,VECTOR * State_Old,VECTOR * k1,DBL Time,MECHSIM_BOUNDING_DATA * Bounding_Data)3423 DBL Mechsim_Heun_Step(VECTOR *Forces, VECTOR *State_Array, VECTOR *State_Old, VECTOR *k1, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
3424 {
3425 int i, j;
3426
3427 /* calculate all forces at y_i */
3428 Mechsim_Forces(Forces, State_Old, Time, Bounding_Data);
3429 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3430
3431 for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
3432 {
3433 j = i % MechsimOptions.Data.mass_count;
3434
3435 if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
3436 {
3437 /* k1 = f(y_old) */
3438 Assign_Vector(k1[i], Forces[i]);
3439
3440 /* y2 = y_old + h*f(y_old) */
3441 State_Array[i][X] = State_Old[i][X] + MechsimOptions.Time_Step*Forces[i][X];
3442 State_Array[i][Y] = State_Old[i][Y] + MechsimOptions.Time_Step*Forces[i][Y];
3443 State_Array[i][Z] = State_Old[i][Z] + MechsimOptions.Time_Step*Forces[i][Z];
3444
3445 Do_Cooperate(1);
3446 }
3447 }
3448
3449 /* =================================================================== */
3450
3451 /* calculate all forces at y2, time = t+h */
3452 Mechsim_Forces(Forces, State_Old, Time+MechsimOptions.Time_Step, Bounding_Data);
3453 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3454
3455 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3456 {
3457 j = i + MechsimOptions.Data.mass_count;
3458
3459 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
3460 {
3461 /* y_new = y_old + (h/2)*(k1 + k2) */
3462 /* k2 = f(y2) */
3463 State_Array[i][X] = State_Old[i][X] + MechsimOptions.Time_Step*0.5*(k1[i][X]+Forces[i][X]);
3464 State_Array[i][Y] = State_Old[i][Y] + MechsimOptions.Time_Step*0.5*(k1[i][Y]+Forces[i][Y]);
3465 State_Array[i][Z] = State_Old[i][Z] + MechsimOptions.Time_Step*0.5*(k1[i][Z]+Forces[i][Z]);
3466
3467 State_Array[j][X] = State_Old[j][X] + MechsimOptions.Time_Step*0.5*(k1[j][X]+Forces[j][X]);
3468 State_Array[j][Y] = State_Old[j][Y] + MechsimOptions.Time_Step*0.5*(k1[j][Y]+Forces[j][Y]);
3469 State_Array[j][Z] = State_Old[j][Z] + MechsimOptions.Time_Step*0.5*(k1[j][Z]+Forces[j][Z]);
3470
3471 Mechsim_Check_Instability(State_Array[i], State_Array[j], Forces[j]);
3472 }
3473 else
3474 {
3475 Assign_Vector(State_Array[i], State_Old[i]);
3476 Make_Vector(State_Array[j], 0.0, 0.0, 0.0);
3477 }
3478 Do_Cooperate(1);
3479 }
3480
3481 /* =================================================================== */
3482
3483 return Time+MechsimOptions.Time_Step;
3484 }
3485
3486
3487 /*****************************************************************************
3488 *
3489 * FUNCTION
3490 *
3491 * Mechsim_Runge_Kutta4_Step
3492 *
3493 * INPUT
3494 *
3495 * Forces - To store the evaluations of the right side of the equation
3496 * State_Array - new system state (does not need to be initialized)
3497 * State_Old - old system state
3498 * k1,k2,k3 - in between evaluations
3499 * Do_k1 - if false k1 is not evaluated (because it has been before)
3500 * Time - current time value
3501 * Time_Step - current time step
3502 * Bounding_Data - bounding information for collisions
3503 *
3504 * OUTPUT
3505 *
3506 * RETURNS
3507 *
3508 * new time = old time + time step
3509 *
3510 * AUTHOR
3511 *
3512 * Christoph Hormann
3513 *
3514 * DESCRIPTION
3515 *
3516 * Runs a single step for numerical integration with 4th order Runge-Kutta method
3517 *
3518 * ===============================================
3519 * 4th order Runge-Kutta method:
3520 * ===============================================
3521 *
3522 * Formula:
3523 *
3524 * y_i+1 = y_i + (h/6)*f(k_1 + 2*k_2 + 2*k_3 + k_4)
3525 *
3526 * with:
3527 *
3528 * k_1 = f(y_i, t)
3529 * k_2 = f(y_i + (h/2)*k_1, t + (h/2))
3530 * k_3 = f(y_i + (h/2)*k_2, t + (h/2))
3531 * k_4 = f(y_i + h*k_3, t + h)
3532 *
3533 * Note: y_i/y_i+1 represents both positions and velocities
3534 *
3535 * CHANGES
3536 *
3537 * --- Apr 2003 : Moved out of Mechsim_Simulate()
3538 * Jan 2005 : Extracted function for single time step
3539 *
3540 ******************************************************************************/
3541
Mechsim_Runge_Kutta4_Step(VECTOR * Forces,VECTOR * State_Array,VECTOR * State_Old,VECTOR * k1,VECTOR * k2,VECTOR * k3,bool Do_k1,DBL Time,DBL Time_Step,MECHSIM_BOUNDING_DATA * Bounding_Data)3542 DBL Mechsim_Runge_Kutta4_Step(VECTOR *Forces, VECTOR *State_Array, VECTOR *State_Old, VECTOR *k1, VECTOR *k2, VECTOR *k3, bool Do_k1, DBL Time, DBL Time_Step, MECHSIM_BOUNDING_DATA* Bounding_Data)
3543 {
3544 int i, j;
3545
3546 /* calculate all forces at y_i */
3547 Mechsim_Forces(Forces, State_Old, Time, Bounding_Data);
3548 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3549
3550 for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
3551 {
3552 j = i % MechsimOptions.Data.mass_count;
3553
3554 if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
3555 {
3556 /* k1 = f(y_old) */
3557 Assign_Vector(k1[i], Forces[i]);
3558
3559 /* y2 = y_old + (h/2)*k1 */
3560 State_Array[i][X] = State_Old[i][X] + Time_Step*0.5*Forces[i][X];
3561 State_Array[i][Y] = State_Old[i][Y] + Time_Step*0.5*Forces[i][Y];
3562 State_Array[i][Z] = State_Old[i][Z] + Time_Step*0.5*Forces[i][Z];
3563
3564 Do_Cooperate(1);
3565 }
3566 }
3567
3568 /* =================================================================== */
3569
3570 /* calculate all forces at y2, time = t + (h/2) */
3571 Mechsim_Forces(Forces, State_Array, Time + Time_Step*0.5, Bounding_Data);
3572 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3573
3574 for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
3575 {
3576 j = i % MechsimOptions.Data.mass_count;
3577
3578 if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
3579 {
3580 /* k2 = f(y_old + (h/2)*k1, t + (h/2)) */
3581 Assign_Vector(k2[i], Forces[i]);
3582
3583 /* y3 = y_old + (h/2)*k_2 */
3584 State_Array[i][X] = State_Old[i][X] + Time_Step*0.5*Forces[i][X];
3585 State_Array[i][Y] = State_Old[i][Y] + Time_Step*0.5*Forces[i][Y];
3586 State_Array[i][Z] = State_Old[i][Z] + Time_Step*0.5*Forces[i][Z];
3587
3588 Do_Cooperate(1);
3589 }
3590 }
3591
3592 /* =================================================================== */
3593
3594 /* calculate all forces at y3, time = t + (h/2) */
3595 Mechsim_Forces(Forces, State_Array, Time + Time_Step*0.5, Bounding_Data);
3596 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3597
3598 for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
3599 {
3600 j = i % MechsimOptions.Data.mass_count;
3601
3602 if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
3603 {
3604 /* k3 = f(y_old + (h/2)*k_2, t + (h/2)) */
3605 Assign_Vector(k3[i], Forces[i]);
3606
3607 /* y4 = y_old + h*k_3 */
3608 State_Array[i][X] = State_Old[i][X] + Time_Step*Forces[i][X];
3609 State_Array[i][Y] = State_Old[i][Y] + Time_Step*Forces[i][Y];
3610 State_Array[i][Z] = State_Old[i][Z] + Time_Step*Forces[i][Z];
3611
3612 Do_Cooperate(1);
3613 }
3614 }
3615
3616 /* =================================================================== */
3617
3618 /* calculate all forces at y4, time = t + h */
3619 Mechsim_Forces(Forces, State_Array, Time + Time_Step, Bounding_Data);
3620 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
3621
3622 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3623 {
3624 j = i + MechsimOptions.Data.mass_count;
3625
3626 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
3627 {
3628 /* y_new = y_old + (h/6)*(k1 + 2*k2 + 2*k3 + k4) */
3629 /* k4 = f(y_old + h*k_3, t + h) */
3630 State_Array[i][X] = State_Old[i][X] + Time_Step*(1.0/6.0)*(k1[i][X]+2.0*k2[i][X]+2.0*k3[i][X]+Forces[i][X]);
3631 State_Array[i][Y] = State_Old[i][Y] + Time_Step*(1.0/6.0)*(k1[i][Y]+2.0*k2[i][Y]+2.0*k3[i][Y]+Forces[i][Y]);
3632 State_Array[i][Z] = State_Old[i][Z] + Time_Step*(1.0/6.0)*(k1[i][Z]+2.0*k2[i][Z]+2.0*k3[i][Z]+Forces[i][Z]);
3633
3634 State_Array[j][X] = State_Old[j][X] + Time_Step*(1.0/6.0)*(k1[j][X]+2.0*k2[j][X]+2.0*k3[j][X]+Forces[j][X]);
3635 State_Array[j][Y] = State_Old[j][Y] + Time_Step*(1.0/6.0)*(k1[j][Y]+2.0*k2[j][Y]+2.0*k3[j][Y]+Forces[j][Y]);
3636 State_Array[j][Z] = State_Old[j][Z] + Time_Step*(1.0/6.0)*(k1[j][Z]+2.0*k2[j][Z]+2.0*k3[j][Z]+Forces[j][Z]);
3637
3638 Mechsim_Check_Instability(State_Array[i], State_Array[j], Forces[j]);
3639 }
3640 else
3641 {
3642 Assign_Vector(State_Array[i], State_Old[i]);
3643 Make_Vector(State_Array[j], 0.0, 0.0, 0.0);
3644 }
3645 Do_Cooperate(1);
3646 }
3647 /* =================================================================== */
3648
3649 return (Time+Time_Step);
3650 }
3651
3652
3653 /*****************************************************************************
3654 *
3655 * FUNCTION
3656 *
3657 * Mechsim_Runge_Kutta4_Step_Adaptive
3658 *
3659 * INPUT
3660 *
3661 * Forces - To store the evaluations of the right side of the equation
3662 * State_Array - new system state (does not need to be initialized)
3663 * State_Old - old system state
3664 * State2, State3 - large and in between step
3665 * k1,k2,k3 - in between evaluations
3666 * Time - current time value
3667 * Bounding_Data - bounding information for collisions
3668 *
3669 * OUTPUT
3670 *
3671 * RETURNS
3672 *
3673 * new time = old time + current time step
3674 *
3675 * AUTHOR
3676 *
3677 * Christoph Hormann
3678 *
3679 * DESCRIPTION
3680 *
3681 * Runs a 4th order Runge-Kutta step and adjusts the time stepping
3682 *
3683 * Idea based on example in 'Numerical Recipes in C'
3684 *
3685 * CHANGES
3686 *
3687 * --- Feb 2005 : Creation
3688 *
3689 ******************************************************************************/
3690
Mechsim_Runge_Kutta4_Step_Adaptive(VECTOR * Forces,VECTOR * State_Array,VECTOR * State_Old,VECTOR * State2,VECTOR * State3,VECTOR * k1,VECTOR * k2,VECTOR * k3,DBL Time,MECHSIM_BOUNDING_DATA * Bounding_Data)3691 DBL Mechsim_Runge_Kutta4_Step_Adaptive(VECTOR *Forces, VECTOR *State_Array, VECTOR *State_Old, VECTOR *State2, VECTOR *State3, VECTOR *k1, VECTOR *k2, VECTOR *k3, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
3692 {
3693 DBL Time_New;
3694 DBL Time_Step;
3695 DBL Err;
3696 DBL Error_max;
3697 int i;
3698 /*
3699
3700 Time_New = Mechsim_Runge_Kutta4_Step(Forces, State_Array, State_Old, k1, k2, k3, true, Time, MechsimOptions.Time_Step, Bounding_Data);
3701
3702 #if(MECHSIM_DEBUG == 1)
3703 Debug_Info("Runge_Kutta4 Non-Adaptive %g\n", MechsimOptions.Time_Step);
3704 #endif
3705
3706 return (Time_New);
3707 */
3708
3709 Time_Step = MechsimOptions.Time_Step;
3710
3711 while (true)
3712 {
3713 /* do integration step (large) */
3714 Time_New = Mechsim_Runge_Kutta4_Step(Forces, State2, State_Old, k1, k2, k3, true, Time, Time_Step*2.0, Bounding_Data);
3715 /* do integration step (2x small) */
3716 Time_New = Mechsim_Runge_Kutta4_Step(Forces, State3, State_Old, k1, k2, k3, false, Time, Time_Step, Bounding_Data);
3717 Time_New = Mechsim_Runge_Kutta4_Step(Forces, State_Array, State3, k1, k2, k3, true, Time_New, Time_Step, Bounding_Data);
3718
3719 Error_max = 0.0;
3720 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3721 {
3722 VDist(Err, State2[i], State_Array[i]);
3723 Error_max = max(Error_max, Err);
3724 }
3725
3726 Error_max /= MechsimOptions.Accuracy;
3727
3728 /* time step small enough */
3729 if ((Error_max <= 1.0) && (Error_max >= 0.0) ) break;
3730
3731 /* otherwise reduce step size */
3732 Time_Step *= max(0.1, Mechsim_Safety_Fact*pow(Error_max, -Mechsim_Exp_Dec));
3733 if (Time_Step < MechsimOptions.Time_Step_Min)
3734 Error("Adaptive time stepping suggested too small time steps (%g).\n", Time_Step);
3735
3736 }
3737
3738 if (Error_max > Mechsim_Error_Threshold) Time_Step *= Mechsim_Safety_Fact*pow(Error_max, -Mechsim_Exp_Inc);
3739 else Time_Step *= 5.0;
3740
3741 if ((Time_Step * 5.0) > (MechsimOptions.End_Time-MechsimOptions.Start_Time))
3742 Time_Step = (MechsimOptions.End_Time-MechsimOptions.Start_Time)/5.0001;
3743
3744 MechsimOptions.Time_Step = Time_Step;
3745
3746 // for stats
3747 MechsimOptions.TS_Min = min(MechsimOptions.TS_Min, Time_Step);
3748 MechsimOptions.TS_Max = max(MechsimOptions.TS_Max, Time_Step);
3749
3750 return (Time_New);
3751 }
3752
3753
3754 /*****************************************************************************
3755 *
3756 * FUNCTION
3757 *
3758 * Mechsim_Integrate_Euler
3759 *
3760 * INPUT
3761 *
3762 * OUTPUT
3763 *
3764 * RETURNS
3765 *
3766 * AUTHOR
3767 *
3768 * Christoph Hormann
3769 *
3770 * DESCRIPTION
3771 *
3772 * Do numerical integration with Euler method.
3773 *
3774 * CHANGES
3775 *
3776 * --- Apr 2003 : Moved out of Mechsim_Simulate()
3777 * Jan 2005 : Modified for new data structures and moved integration step
3778 * into separate function
3779 * Apr 2005 : moved out Forces allocation
3780 *
3781 ******************************************************************************/
3782
Mechsim_Integrate_Euler(VECTOR * State_Array,VECTOR * Forces)3783 void Mechsim_Integrate_Euler(VECTOR *State_Array, VECTOR *Forces)
3784 {
3785 int i, j;
3786
3787 VECTOR *State_Old;
3788 VECTOR *Pos_Array = State_Array;
3789 VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
3790 VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];
3791
3792 VECTOR *Attach_Move = NULL;
3793 VECTOR *Attach_Move_Vel = NULL;
3794
3795 int param;
3796
3797 MECHSIM_BOUNDING_DATA* Bounding_Data;
3798
3799 Bounding_Data = init_bounding();
3800
3801 if (MechsimOptions.Attachments.count>0)
3802 {
3803 /* Attachments movement data cache */
3804 Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
3805 Attach_Move_Vel = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
3806 }
3807
3808 /* Allocate additional data blocks for integration */
3809 State_Old = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
3810
3811 MechsimOptions.Time = MechsimOptions.Start_Time;
3812 MechsimOptions.Steps=0;
3813
3814 while (MechsimOptions.Time < MechsimOptions.End_Time)
3815 {
3816 Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
3817 Do_Cooperate(0);
3818
3819 POV_MEMCPY(State_Old, State_Array, MechsimOptions.Data.mass_count*2*sizeof (VECTOR));
3820
3821 /* do integration step */
3822 MechsimOptions.Time = Mechsim_Euler_Step(Forces, State_Array, State_Old, MechsimOptions.Time, Bounding_Data);
3823 MechsimOptions.Steps++;
3824
3825 /* calculate the movement of the attachments during this time step */
3826 if (MechsimOptions.Attachments.count>0)
3827 {
3828 for (i=0; i<MechsimOptions.Attachments.count; i++)
3829 {
3830 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));
3831
3832 POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
3833 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
3834 for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);
3835
3836 POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
3837 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
3838 for(param = 0; param < min((int)f->return_size, 3); param++)
3839 {
3840 Attach_Move[i][param] -= POVFPU_GetLocal(param);
3841 Attach_Move_Vel[i][param] = Attach_Move[i][param]/MechsimOptions.Time_Step;
3842 }
3843 }
3844 }
3845
3846 /* apply attachments and penetration prevention for environments */
3847 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3848 {
3849 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
3850 {
3851 j = i + MechsimOptions.Data.mass_count;
3852
3853 Do_Cooperate(1);
3854
3855 if (MechsimOptions.Data.Masses[i].Attach >= 0)
3856 {
3857 /* move the attached masses */
3858 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][0])
3859 {
3860 State_Array[i][X] = State_Old[i][X] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][X];
3861 State_Array[j][X] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][X];
3862 }
3863 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][1])
3864 {
3865 State_Array[i][Y] = State_Old[i][Y] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Y];
3866 State_Array[j][Y] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Y];
3867 }
3868 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][2])
3869 {
3870 State_Array[i][Z] = State_Old[i][Z] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Z];
3871 State_Array[j][Z] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Z];
3872 }
3873 }
3874
3875 Mechsim_Check_Col(State_Array[i], State_Old[i]);
3876 }
3877 }
3878
3879 /* integrate the inner variables of viscoelastic elements */
3880 if (MechsimOptions.Data.ve_count>0) Mechsim_Integrate_Viscoelastics(State_Array);
3881
3882 Increase_Counter(stats[MechSim_Steps]);
3883
3884 }
3885
3886 POV_FREE(State_Old);
3887
3888 if (MechsimOptions.Attachments.count>0)
3889 {
3890 POV_FREE(Attach_Move);
3891 POV_FREE(Attach_Move_Vel);
3892 }
3893
3894 free_bounding(Bounding_Data);
3895
3896 }
3897
3898
3899 /*****************************************************************************
3900 *
3901 * FUNCTION
3902 *
3903 * Mechsim_Integrate_Heun
3904 *
3905 * INPUT
3906 *
3907 * OUTPUT
3908 *
3909 * RETURNS
3910 *
3911 * AUTHOR
3912 *
3913 * Christoph Hormann
3914 *
3915 * DESCRIPTION
3916 *
3917 * Do numerical integration with Heun method.
3918 *
3919 * CHANGES
3920 *
3921 * --- Apr 2003 : Moved out of Mechsim_Simulate()
3922 * Jan 2005 : Modified for new data structures
3923 * Apr 2005 : moved out Forces allocation
3924 *
3925 ******************************************************************************/
3926
Mechsim_Integrate_Heun(VECTOR * State_Array,VECTOR * Forces)3927 void Mechsim_Integrate_Heun(VECTOR *State_Array, VECTOR *Forces)
3928 {
3929 int i, j;
3930
3931 VECTOR *State_Old;
3932 VECTOR *k1;
3933
3934 VECTOR *Pos_Array = State_Array;
3935 VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
3936 VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];
3937
3938 VECTOR *Attach_Move = NULL;
3939 VECTOR *Attach_Move_Vel = NULL;
3940
3941 int param;
3942
3943 MECHSIM_BOUNDING_DATA* Bounding_Data;
3944
3945 Bounding_Data = init_bounding();
3946
3947 /* Attachments movement data cache */
3948 if (MechsimOptions.Attachments.count>0)
3949 {
3950 Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
3951 Attach_Move_Vel = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
3952 }
3953
3954 /* Allocate additional data blocks for integration */
3955 State_Old = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
3956 k1 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
3957
3958 while (MechsimOptions.Time < MechsimOptions.End_Time)
3959 {
3960 Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
3961 Do_Cooperate(0);
3962
3963 POV_MEMCPY(State_Old, State_Array, MechsimOptions.Data.mass_count*2*sizeof (VECTOR));
3964
3965 /* do integration step */
3966 MechsimOptions.Time = Mechsim_Heun_Step(Forces, State_Array, State_Old, k1, MechsimOptions.Time, Bounding_Data);
3967 MechsimOptions.Steps++;
3968
3969 /* calculate the movement of the attachments during this time step */
3970 if (MechsimOptions.Attachments.count>0)
3971 {
3972 for (i=0; i<MechsimOptions.Attachments.count; i++)
3973 {
3974 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));
3975
3976 POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
3977 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
3978 for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);
3979
3980 POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
3981 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
3982 for(param = 0; param < min((int)f->return_size, 3); param++)
3983 {
3984 Attach_Move[i][param] -= POVFPU_GetLocal(param);
3985 Attach_Move_Vel[i][param] = Attach_Move[i][param]/MechsimOptions.Time_Step;
3986 }
3987 }
3988 }
3989
3990 /* apply attachments and penetration prevention for environments */
3991 for (i=0; i<MechsimOptions.Data.mass_count; i++)
3992 {
3993 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
3994 {
3995 j = i + MechsimOptions.Data.mass_count;
3996
3997 Do_Cooperate(1);
3998
3999 if (MechsimOptions.Data.Masses[i].Attach >= 0)
4000 {
4001 /* move the attached masses */
4002 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][0])
4003 {
4004 State_Array[i][X] = State_Old[i][X] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][X];
4005 State_Array[j][X] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][X];
4006 }
4007 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][1])
4008 {
4009 State_Array[i][Y] = State_Old[i][Y] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Y];
4010 State_Array[j][Y] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Y];
4011 }
4012 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][2])
4013 {
4014 State_Array[i][Z] = State_Old[i][Z] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Z];
4015 State_Array[j][Z] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Z];
4016 }
4017 }
4018
4019 Mechsim_Check_Col(State_Array[i], State_Old[i]);
4020 }
4021 }
4022 /* =================================================================== */
4023
4024 // integrate the inner variables of viscoelastic elements
4025 if (MechsimOptions.Data.ve_count>0) Mechsim_Integrate_Viscoelastics(State_Array);
4026
4027 Increase_Counter(stats[MechSim_Steps]);
4028
4029 }
4030
4031 POV_FREE(k1);
4032 POV_FREE(State_Old);
4033
4034 if (MechsimOptions.Attachments.count>0)
4035 {
4036 POV_FREE(Attach_Move);
4037 POV_FREE(Attach_Move_Vel);
4038 }
4039
4040 free_bounding(Bounding_Data);
4041
4042 }
4043
4044
4045 /*****************************************************************************
4046 *
4047 * FUNCTION
4048 *
4049 * Mechsim_Integrate_Runge_Kutta4
4050 *
4051 * INPUT
4052 *
4053 * OUTPUT
4054 *
4055 * RETURNS
4056 *
4057 * AUTHOR
4058 *
4059 * Christoph Hormann
4060 *
4061 * DESCRIPTION
4062 *
4063 * Do numerical integration with 4th order Runge Kutta method.
4064 *
4065 * CHANGES
4066 *
4067 * --- Apr 2003 : Moved out of Mechsim_Simulate()
4068 * Jan 2005 : Modified for new data structures
4069 * Apr 2005 : moved out Forces allocation
4070 *
4071 ******************************************************************************/
4072
Mechsim_Integrate_Runge_Kutta4(VECTOR * State_Array,VECTOR * Forces)4073 void Mechsim_Integrate_Runge_Kutta4(VECTOR *State_Array, VECTOR *Forces)
4074 {
4075 int i, j;
4076
4077 VECTOR *State_Old;
4078 VECTOR *State2;
4079 VECTOR *State3;
4080 VECTOR *k1;
4081 VECTOR *k2;
4082 VECTOR *k3;
4083
4084 VECTOR *Pos_Array = State_Array;
4085 VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
4086 VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];
4087
4088 VECTOR *Attach_Move = NULL;
4089 VECTOR *Attach_Move_Vel = NULL;
4090
4091 int param;
4092
4093 MECHSIM_BOUNDING_DATA* Bounding_Data;
4094
4095 Bounding_Data = init_bounding();
4096
4097 /* Attachments movement data cache */
4098 if (MechsimOptions.Attachments.count>0)
4099 {
4100 Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
4101 Attach_Move_Vel = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
4102 }
4103
4104 /* Allocate additional data blocks for integration */
4105 State_Old = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4106 State2 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4107 State3 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4108 k1 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4109 k2 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4110 k3 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4111
4112 while (MechsimOptions.Time < MechsimOptions.End_Time)
4113 {
4114 Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
4115 Do_Cooperate(0);
4116
4117 POV_MEMCPY(State_Old, State_Array, MechsimOptions.Data.mass_count*2*sizeof (VECTOR));
4118
4119 /* do integration step (adaptive) */
4120 //Time = Mechsim_Runge_Kutta4_Step(Forces, State_Array, State_Old, k1, k2, k3, true, MechsimOptions.Time, MechsimOptions.Time_Step, Bounding_Data);
4121 MechsimOptions.Time = Mechsim_Runge_Kutta4_Step_Adaptive(Forces, State_Array, State_Old, State2, State3, k1, k2, k3, MechsimOptions.Time, Bounding_Data);
4122 MechsimOptions.Steps++;
4123
4124 /* calculate the movement of the attachments during this time step */
4125 if (MechsimOptions.Attachments.count>0)
4126 {
4127 for (i=0; i<MechsimOptions.Attachments.count; i++)
4128 {
4129 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));
4130
4131 POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
4132 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
4133 for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);
4134
4135 POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
4136 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
4137 for(param = 0; param < min((int)f->return_size, 3); param++)
4138 {
4139 Attach_Move[i][param] -= POVFPU_GetLocal(param);
4140 Attach_Move_Vel[i][param] = Attach_Move[i][param]/MechsimOptions.Time_Step;
4141 }
4142 }
4143 }
4144
4145 /* apply attachments and penetration prevention for environments */
4146 for (i=0; i<MechsimOptions.Data.mass_count; i++)
4147 {
4148 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
4149 {
4150 if (MechsimOptions.Data.Masses[i].Attach >= 0)
4151 {
4152 j = i + MechsimOptions.Data.mass_count;
4153
4154 Do_Cooperate(1);
4155
4156 /* move the attached masses */
4157 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][0])
4158 {
4159 State_Array[i][X] = State_Old[i][X] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][X];
4160 State_Array[j][X] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][X];
4161 }
4162 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][1])
4163 {
4164 State_Array[i][Y] = State_Old[i][Y] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Y];
4165 State_Array[j][Y] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Y];
4166 }
4167 if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][2])
4168 {
4169 State_Array[i][Z] = State_Old[i][Z] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Z];
4170 State_Array[j][Z] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Z];
4171 }
4172 }
4173
4174 Mechsim_Check_Col(State_Array[i], State_Old[i]);
4175 }
4176 }
4177 /* =================================================================== */
4178
4179 // integrate the inner variables of viscoelastic elements
4180 if (MechsimOptions.Data.ve_count>0) Mechsim_Integrate_Viscoelastics(State_Array);
4181
4182 Increase_Counter(stats[MechSim_Steps]);
4183
4184 }
4185
4186 POV_FREE(k1);
4187 POV_FREE(k2);
4188 POV_FREE(k3);
4189 POV_FREE(State_Old);
4190 POV_FREE(State2);
4191 POV_FREE(State3);
4192
4193 if (MechsimOptions.Attachments.count>0)
4194 {
4195 POV_FREE(Attach_Move);
4196 POV_FREE(Attach_Move_Vel);
4197 }
4198
4199 free_bounding(Bounding_Data);
4200
4201 }
4202
4203
4204 /*****************************************************************************
4205 *
4206 * FUNCTION
4207 *
4208 * Mechsim_Integrate_Gradient
4209 *
4210 * INPUT
4211 *
4212 * OUTPUT
4213 *
4214 * RETURNS
4215 *
4216 * AUTHOR
4217 *
4218 * Christoph Hormann
4219 *
4220 * DESCRIPTION
4221 *
4222 * Do Gradient descent method
4223 *
4224 * ===============================================
4225 * Gradient-descent-Method:
4226 * ===============================================
4227 *
4228 * CHANGES
4229 *
4230 * --- Apr 2003 : Moved out of Mechsim_Simulate()
4231 * Jan 2005 : Modified for new data structures
4232 * Apr 2005 : moved out Accel allocation
4233 *
4234 ******************************************************************************/
4235
Mechsim_Integrate_Gradient(VECTOR * State_Array,VECTOR * Forces)4236 void Mechsim_Integrate_Gradient(VECTOR *State_Array, VECTOR *Forces)
4237 {
4238 int i, j;
4239
4240 VECTOR *Pos_Array = State_Array;
4241 VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
4242 VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];
4243 VECTOR *Attach_Move = NULL;
4244
4245 int param;
4246
4247 MECHSIM_BOUNDING_DATA* Bounding_Data;
4248
4249 Bounding_Data = init_bounding();
4250
4251 /* Attachments movement data cache */
4252 if (MechsimOptions.Attachments.count>0)
4253 Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
4254
4255 POV_MEMCPY(Vel_Array, Pos_Array, MechsimOptions.Data.mass_count*sizeof (VECTOR));
4256
4257 while (MechsimOptions.Time < MechsimOptions.End_Time)
4258 {
4259 Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
4260 Do_Cooperate(0);
4261
4262 /* calculate the movement of the attachments during this time step */
4263 if (MechsimOptions.Attachments.count>0)
4264 {
4265 for (i=0; i<MechsimOptions.Attachments.count; i++)
4266 {
4267 FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));
4268
4269 POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
4270 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
4271 for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);
4272
4273 POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
4274 (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
4275 for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] -= POVFPU_GetLocal(param);
4276 }
4277 }
4278
4279 /* calculate all gradients */
4280 Mechsim_Gradients(Accel, Pos_Array, Vel_Array, MechsimOptions.Time, Bounding_Data);
4281 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;
4282
4283 Do_Cooperate(1);
4284
4285 for (i=0; i<MechsimOptions.Data.mass_count; i++)
4286 {
4287 if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
4288 {
4289
4290 // store last position to allow object intersection based environments
4291 // see function 'Mechsim_Gradient_Collide_Environment()' for how this is used
4292 Assign_Vector(Vel_Array[i], Pos_Array[i]);
4293
4294 /* move masses in direction of the gradient */
4295 Pos_Array[i][X] = Pos_Array[i][X] + MechsimOptions.Time_Step*Accel[i][X];
4296 Pos_Array[i][Y] = Pos_Array[i][Y] + MechsimOptions.Time_Step*Accel[i][Y];
4297 Pos_Array[i][Z] = Pos_Array[i][Z] + MechsimOptions.Time_Step*Accel[i][Z];
4298
4299 Do_Cooperate(1);
4300
4301 if (MechsimOptions.Data.Masses[i].Attach >= 0)
4302 {
4303 /* move the attached masses */
4304 VAdd(Pos_Array[i], Vel_Array[i], Attach_Move[MechsimOptions.Data.Masses[i].Attach]);
4305 }
4306
4307 Mechsim_Check_Instability(Pos_Array[i], Accel[i], Accel[i]);
4308 Mechsim_Check_Col(Pos_Array[i], Vel_Array[i]);
4309 }
4310 }
4311
4312 MechsimOptions.Time += MechsimOptions.Time_Step;
4313 MechsimOptions.Steps++;
4314
4315 Increase_Counter(stats[MechSim_Steps]);
4316
4317 }
4318
4319 POV_FREE(Accel);
4320
4321 if (MechsimOptions.Attachments.count>0)
4322 POV_FREE(Attach_Move);
4323
4324 // make sure we save zero velocity to file
4325 for (i=0; i<MechsimOptions.Data.mass_count; i++)
4326 {
4327 Vel_Array[i][X] = 0.0;
4328 Vel_Array[i][Y] = 0.0;
4329 Vel_Array[i][Z] = 0.0;
4330 }
4331
4332 free_bounding(Bounding_Data);
4333
4334 }
4335
4336
4337 /*****************************************************************************
4338 *
4339 * FUNCTION
4340 *
4341 * Mechsim_Simulate
4342 *
4343 * INPUT
4344 *
4345 * OUTPUT
4346 *
4347 * RETURNS
4348 *
4349 * AUTHOR
4350 *
4351 * Christoph Hormann
4352 *
4353 * DESCRIPTION
4354 *
4355 * Do the actual mechanics simulation
4356 *
4357 * CHANGES
4358 *
4359 * --- Aug 2002 : Creation
4360 *
4361 ******************************************************************************/
4362
Mechsim_Simulate(void)4363 void Mechsim_Simulate(void)
4364 {
4365 int i;
4366
4367 VECTOR *State_Array;
4368 VECTOR *Forces;
4369
4370 if ((MechsimOptions.Timing_Given || MechsimOptions.Timing_Loaded) &&
4371 (MechsimOptions.Start_Time < MechsimOptions.End_Time))
4372 {
4373
4374 /* Bounding system by Daniel Jungmann */
4375 if ((MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_AUTO) &&
4376 (MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_NO) &&
4377 (MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_BBOX) &&
4378 (MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_HASH))
4379 {
4380 Error("mechanics simulation bounding method %d not supported", MechsimOptions.Bounding);
4381 }
4382
4383 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_AUTO)
4384 {
4385 /* Just turn bounding on if it would be faster */
4386 if (
4387 ((MechsimOptions.Data.max_masses < MECHSIM_BOUNDING_HASH_THRESHOLD) ||
4388 (MechsimOptions.Data.max_faces < MECHSIM_BOUNDING_HASH_THRESHOLD)) &&
4389 (MechsimOptions.Data.max_masses*MechsimOptions.Data.max_faces < (MECHSIM_BOUNDING_HASH_THRESHOLD*MECHSIM_BOUNDING_HASH_THRESHOLD))
4390 )
4391 {
4392 if (
4393 ((MechsimOptions.Data.max_masses < MECHSIM_BOUNDING_BOX_THRESHOLD) ||
4394 (MechsimOptions.Data.max_faces < MECHSIM_BOUNDING_BOX_THRESHOLD)) &&
4395 (MechsimOptions.Data.max_masses*MechsimOptions.Data.max_faces < (MECHSIM_BOUNDING_BOX_THRESHOLD*MECHSIM_BOUNDING_BOX_THRESHOLD))
4396 )
4397 {
4398 MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_NO;
4399 }
4400 else
4401 {
4402 MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_BBOX;
4403 }
4404 }
4405 else MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_HASH;
4406 }
4407
4408 MechsimOptions.TS_Min = MechsimOptions.TS_Max = MechsimOptions.Time_Step;
4409 MechsimOptions.Time = MechsimOptions.Start_Time;
4410 MechsimOptions.Steps=0;
4411
4412 Send_Progress("Calculating Mechanics Simulation", PROGRESS_MECHANICS_SIMULATION);
4413
4414 START_TIME
4415
4416 /* fill the group data structures */
4417 Mechsim_Prepare_Data();
4418
4419 /* Allocate data blocks for integration */
4420 State_Array = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4421 Forces = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
4422
4423 /* Read data from MechsimOptions.Data */
4424 for (i=0; i<MechsimOptions.Data.mass_count; i++)
4425 {
4426 Assign_Vector(State_Array[i], MechsimOptions.Data.Masses[i].Position);
4427 Assign_Vector(State_Array[i+MechsimOptions.Data.mass_count], MechsimOptions.Data.Masses[i].Velocity);
4428 }
4429
4430 #if(MECHSIM_DEBUG == 1)
4431 Debug_Info("==========================================================\n");
4432 Debug_Info("Mechsim Options:\n");
4433
4434 Debug_Info(" MechsimOptions.Method: %d\n", MechsimOptions.Method);
4435 Debug_Info(" MechsimOptions.Step_Count: %d\n", MechsimOptions.Step_Count);
4436 Debug_Info(" MechsimOptions.Time_Step: %g\n", MechsimOptions.Time_Step);
4437 Debug_Info(" MechsimOptions.Start_Time: %g\n", MechsimOptions.Start_Time);
4438 Debug_Info(" MechsimOptions.End_Time: %g\n", MechsimOptions.End_Time);
4439 Debug_Info(" MechsimOptions.Timing_Given: %d\n", MechsimOptions.Timing_Given);
4440
4441 // for adaptive time stepping
4442 Debug_Info(" MechsimOptions.Accuracy: %g\n", MechsimOptions.Accuracy);
4443 Debug_Info(" MechsimOptions.Time_Step_Min: %g\n", MechsimOptions.Time_Step_Min);
4444
4445 // For message output
4446 Debug_Info(" MechsimOptions.Steps: %d\n", MechsimOptions.Steps);
4447 Debug_Info(" MechsimOptions.Time: %g\n", MechsimOptions.Time);
4448
4449 Debug_Info(" MechsimOptions.gravity[Z]: %g\n", MechsimOptions.gravity[Z]);
4450
4451 Debug_Info(" MechsimOptions.Enabled: %d\n", MechsimOptions.Enabled);
4452
4453 Debug_Info(" MechsimOptions.Bounding: %d\n", MechsimOptions.Bounding);
4454
4455 Debug_Info(" MechsimOptions.Data.mass_count: %d\n", MechsimOptions.Data.mass_count);
4456 Debug_Info(" MechsimOptions.Data.connect_count: %d\n", MechsimOptions.Data.connect_count);
4457 Debug_Info(" MechsimOptions.Data.faces_count: %d\n", MechsimOptions.Data.faces_count);
4458 Debug_Info(" MechsimOptions.Data.ve_count: %d\n", MechsimOptions.Data.ve_count);
4459 Debug_Info(" MechsimOptions.Env_count: %d\n", MechsimOptions.Env_count);
4460 Debug_Info(" MechsimOptions.Env_max_count: %d\n", MechsimOptions.Env_max_count);
4461
4462 Debug_Info("==========================================================\n");
4463 #endif
4464
4465
4466 switch (MechsimOptions.Method)
4467 {
4468 case MECHSIM_METHOD_EULER: // ---- EULER ----
4469 Mechsim_Integrate_Euler(State_Array, Forces);
4470 break;
4471
4472 case MECHSIM_METHOD_HEUN: // ---- HEUN ----
4473 Mechsim_Integrate_Heun(State_Array, Forces);
4474 break;
4475
4476 case MECHSIM_METHOD_RUNGE_KUTTA4: // ---- 4th ORDER RUNGE KUTTA ----
4477 Mechsim_Integrate_Runge_Kutta4(State_Array, Forces);
4478 break;
4479
4480 case MECHSIM_METHOD_GRADIENT:
4481 Mechsim_Integrate_Gradient(State_Array, Forces);
4482 break;
4483
4484 default:
4485 Error("mechanics simulation method %d not supported", MechsimOptions.Method);
4486 break;
4487 }
4488
4489 Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION, 0);
4490 Do_Cooperate(0);
4491
4492 /* Write data back to MechsimOptions.Data */
4493 for (i=0; i<MechsimOptions.Data.mass_count; i++)
4494 {
4495 Assign_Vector(MechsimOptions.Data.Masses[i].Position, State_Array[i]);
4496 Assign_Vector(MechsimOptions.Data.Masses[i].Velocity, State_Array[i+MechsimOptions.Data.mass_count]);
4497 Assign_Vector(MechsimOptions.Data.Masses[i].Acceleration, Forces[i+MechsimOptions.Data.mass_count]);
4498 }
4499
4500
4501 POV_FREE(State_Array);
4502 POV_FREE(Forces);
4503
4504 STOP_TIME
4505 tmechsim = TIME_ELAPSED;
4506 tmechsim_frame = tmechsim;
4507 tmechsim_total += tmechsim;
4508
4509 }
4510
4511 if (MechsimOptions.Save_File) Mechsim_save_file();
4512
4513 //back to parsing
4514 Send_Progress("Parsing", PROGRESS_PARSING);
4515
4516 }
4517
4518
4519 /*****************************************************************************
4520 *
4521 * FUNCTION
4522 *
4523 * Mechsim_Create_Group
4524 *
4525 * INPUT
4526 *
4527 * Count - number of groups to create
4528 *
4529 * OUTPUT
4530 *
4531 * AUTHOR
4532 *
4533 * Christoph Hormann
4534 *
4535 * DESCRIPTION
4536 *
4537 * Create new group data structures
4538 *
4539 * CHANGES
4540 *
4541 * --- Sep 2002 : Creation.
4542 *
4543 ******************************************************************************/
Mechsim_Create_Groups(int Count)4544 void Mechsim_Create_Groups(int Count)
4545 {
4546 int i;
4547
4548 if (MechsimOptions.Data.max_groups==0) // first time
4549 {
4550 MechsimOptions.Data.max_groups = Count;
4551 MechsimOptions.Data.Groups = (MECHSIM_GROUP *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof (MECHSIM_GROUP), "mechanics simulation group data block");
4552 }
4553 else
4554 {
4555 if (MechsimOptions.Data.max_groups+Count >= INT_MAX)
4556 {
4557 Error("Too many groups in simulation data");
4558 }
4559 MechsimOptions.Data.max_groups += Count;
4560 MechsimOptions.Data.Groups = (MECHSIM_GROUP *)POV_REALLOC(MechsimOptions.Data.Groups, MechsimOptions.Data.max_groups*sizeof (MECHSIM_GROUP), "mechanics simulation group data block");
4561 }
4562
4563 // for the start allocate space for 20 masses
4564 for (i=MechsimOptions.Data.max_groups-Count; i<MechsimOptions.Data.max_groups; i++)
4565 {
4566 MechsimOptions.Data.Groups[i].max_masses = 20;
4567 MechsimOptions.Data.Groups[i].max_connects = 20;
4568 MechsimOptions.Data.Groups[i].max_faces = 20;
4569
4570 MechsimOptions.Data.Groups[i].Masses = (MECHSIM_MASS **)POV_MALLOC(MechsimOptions.Data.Groups[i].max_masses*sizeof (MECHSIM_MASS *), "mechanics simulation mass pointer data block");
4571 MechsimOptions.Data.Groups[i].Connects = (MECHSIM_CONNECT **)POV_MALLOC(MechsimOptions.Data.Groups[i].max_connects*sizeof (MECHSIM_CONNECT *), "mechanics simulation connection pointer data block");
4572 MechsimOptions.Data.Groups[i].Faces = (MECHSIM_FACE **)POV_MALLOC(MechsimOptions.Data.Groups[i].max_faces*sizeof (MECHSIM_FACE *), "mechanics simulation face pointer data block");
4573
4574 MechsimOptions.Data.Groups[i].mass_count = 0;
4575 MechsimOptions.Data.Groups[i].connect_count = 0;
4576 MechsimOptions.Data.Groups[i].faces_count = 0;
4577 }
4578
4579 }
4580
4581
4582 /*****************************************************************************
4583 *
4584 * FUNCTION
4585 *
4586 * Mechsim_Prepare_Data
4587 *
4588 * INPUT
4589 *
4590 * OUTPUT
4591 *
4592 * AUTHOR
4593 *
4594 * Christoph Hormann
4595 *
4596 * DESCRIPTION
4597 *
4598 * Prepares additional Simulation data structures (groups)
4599 *
4600 * The groups are arrays of structures like the 'Masses', 'Connects' and
4601 * 'Faces' fields. The group structure contains arrays of pointers to
4602 * the actual data. The purpose of all this is to speed up the collision
4603 * calculations.
4604 *
4605 * (Data)
4606 * |_______(Masses) -> array of MECHSIM_MASS
4607 * | pointers to
4608 * |_______(Connects) -> array of MECHSIM_CONNECT <------------
4609 * | |
4610 * |_______(Faces) -> array of MECHSIM_FACE |
4611 * | |
4612 * |_______(Groups) -> array of MECHSIM_GROUP |
4613 * | |
4614 * |_______(Masses) -> array of (MECHSIM_MASS *) | |
4615 * | | |
4616 * |_______(Connects) -> array of (MECHSIM_CONNECT *) |-----
4617 * | |
4618 * |_______(Faces) -> array of (MECHSIM_FACE *) |
4619 *
4620 *
4621 *
4622 *
4623 * CHANGES
4624 *
4625 * --- Sep 2002 : Creation.
4626 *
4627 ******************************************************************************/
Mechsim_Prepare_Data(void)4628 void Mechsim_Prepare_Data(void)
4629 {
4630 int i, Group;
4631
4632 /*------ masses ------*/
4633 for (i=0; i<MechsimOptions.Data.mass_count; i++)
4634 {
4635 Group = MechsimOptions.Data.Masses[i].Group;
4636
4637 // resize group array if necessary
4638 if ( Group >= MechsimOptions.Data.max_groups )
4639 {
4640 Mechsim_Create_Groups(Group-MechsimOptions.Data.max_groups+1);
4641 }
4642
4643 if ( Group >= MechsimOptions.Data.group_count ) MechsimOptions.Data.group_count = Group+1;
4644
4645 // resize mass pointer arrays of the group if necessary
4646 if ( MechsimOptions.Data.Groups[Group].mass_count >= MechsimOptions.Data.Groups[Group].max_masses )
4647 {
4648 if (MechsimOptions.Data.Groups[Group].max_masses >= INT_MAX/2)
4649 {
4650 Error("Too many masses in simulation data");
4651 }
4652 MechsimOptions.Data.Groups[Group].max_masses *= 2;
4653 MechsimOptions.Data.Groups[Group].Masses =
4654 (MECHSIM_MASS **)POV_REALLOC(MechsimOptions.Data.Groups[Group].Masses,
4655 MechsimOptions.Data.Groups[Group].max_masses*
4656 sizeof (MECHSIM_MASS *), "mechanics simulation mass pointer data block");
4657 }
4658
4659 // set mass pointer
4660 MechsimOptions.Data.Groups[Group].Masses[MechsimOptions.Data.Groups[Group].mass_count] = &MechsimOptions.Data.Masses[i];
4661
4662 // set counter
4663 MechsimOptions.Data.Groups[Group].mass_count++;
4664
4665 }
4666
4667 /*------ connects ------*/
4668 for (i=0; i<MechsimOptions.Data.connect_count; i++)
4669 {
4670 Group = MechsimOptions.Data.Connects[i].Group;
4671
4672 // resize group array if necessary
4673 if ( Group >= MechsimOptions.Data.max_groups )
4674 {
4675 Mechsim_Create_Groups(Group-MechsimOptions.Data.max_groups+1);
4676 }
4677
4678 if ( Group >= MechsimOptions.Data.group_count ) MechsimOptions.Data.group_count = Group+1;
4679
4680 // resize connect pointer arrays of the group if necessary
4681 if ( MechsimOptions.Data.Groups[Group].connect_count >= MechsimOptions.Data.Groups[Group].max_connects )
4682 {
4683 if (MechsimOptions.Data.Groups[Group].max_connects >= INT_MAX/2)
4684 {
4685 Error("Too many connects in simulation data");
4686 }
4687 MechsimOptions.Data.Groups[Group].max_connects *= 2;
4688 MechsimOptions.Data.Groups[Group].Connects =
4689 (MECHSIM_CONNECT **)POV_REALLOC(MechsimOptions.Data.Groups[Group].Connects,
4690 MechsimOptions.Data.Groups[Group].max_connects*
4691 sizeof (MECHSIM_CONNECT *), "mechanics simulation connect pointer data block");
4692 }
4693
4694 // set connect pointer
4695 MechsimOptions.Data.Groups[Group].Connects[MechsimOptions.Data.Groups[Group].connect_count] = &MechsimOptions.Data.Connects[i];
4696
4697 // set counter
4698 MechsimOptions.Data.Groups[Group].connect_count++;
4699
4700 }
4701
4702 /*------ faces ------*/
4703 for (i=0; i<MechsimOptions.Data.faces_count; i++)
4704 {
4705 Group = MechsimOptions.Data.Faces[i].Group;
4706
4707 // resize group array if necessary
4708 if ( Group >= MechsimOptions.Data.max_groups )
4709 {
4710 Mechsim_Create_Groups(Group-MechsimOptions.Data.max_groups+1);
4711 }
4712
4713 if ( Group >= MechsimOptions.Data.group_count ) MechsimOptions.Data.group_count = Group+1;
4714
4715 // resize connect pointer arrays of the group if necessary
4716 if ( MechsimOptions.Data.Groups[Group].faces_count >= MechsimOptions.Data.Groups[Group].max_faces )
4717 {
4718 if (MechsimOptions.Data.Groups[Group].max_faces >= INT_MAX/2)
4719 {
4720 Error("Too many faces in simulation data");
4721 }
4722 MechsimOptions.Data.Groups[Group].max_faces *= 2;
4723 MechsimOptions.Data.Groups[Group].Faces =
4724 (MECHSIM_FACE **)POV_REALLOC(MechsimOptions.Data.Groups[Group].Faces,
4725 MechsimOptions.Data.Groups[Group].max_faces*
4726 sizeof (MECHSIM_FACE *), "mechanics simulation face pointer data block");
4727 }
4728
4729 // set connect pointer
4730 MechsimOptions.Data.Groups[Group].Faces[MechsimOptions.Data.Groups[Group].faces_count] = &MechsimOptions.Data.Faces[i];
4731
4732 // set counter
4733 MechsimOptions.Data.Groups[Group].faces_count++;
4734
4735 }
4736
4737
4738 }
4739
4740
4741 /*****************************************************************************
4742 *
4743 * FUNCTION
4744 *
4745 * Mechsim_read_file
4746 *
4747 * INPUT
4748 * file descriptor handle of file (already opened).
4749 * Group - the group the elements should be sorted into
4750 * if zero use groups as in file
4751 *
4752 * OUTPUT
4753 *
4754 * RETURNS - Success 1 / failure 0
4755 *
4756 * AUTHOR
4757 *
4758 * Christoph Hormann
4759 *
4760 * DESCRIPTION
4761 *
4762 * Read in a mechanics simulation data file
4763 *
4764 * CHANGES
4765 *
4766 * --- Aug 2002 : Creation based on radiosity data read function.
4767 * Feb 2005 : Adding new, more universal file format
4768 * Apr 2005 : Adding Force index
4769 *
4770 ******************************************************************************/
Mechsim_read_file(IStream * fd,int Group)4771 bool Mechsim_read_file(IStream *fd, int Group)
4772 {
4773 bool got_eof=false;
4774 long line_num, goodreads;
4775 bool goodparse, parse_err = false;
4776 double Param1_DBL, Param2_DBL;
4777 int Param1_Int;
4778 int fformat, count, i;
4779 char file_id[5];
4780 char line[MAX_STR+2];
4781 char line_param[MAX_STR+2];
4782
4783 int mass_count, connect_count, faces_count, ve_count;
4784 char *Pos;
4785 VECTOR Posi, Vel;
4786 double Rad, Mass, Stiff, Damp, Length;
4787 int Idx1, Idx2, Idx3, Flag, Force, Grp, Attach, ElCnt, Acc;
4788 int prev_mass_count;
4789
4790 if ( fd != NULL )
4791 {
4792 Send_Progress("Reading Mechsim data from file", PROGRESS_MECHSIM_LOADFILE);
4793
4794 if ( fd->getline (line, MAX_STR).eof () )
4795 {
4796 Warning(0, "Error reading mechanics simulation data (unexpected file end)");
4797 return false;
4798 }
4799
4800 goodreads = 0;
4801 goodparse = true;
4802
4803 if (sscanf(line, "%4s, %d,\n", file_id, &fformat) == 2)
4804 if ((file_id[0] == 'M') &&
4805 (file_id[1] == 'S') &&
4806 (file_id[2] == 'I') &&
4807 (file_id[3] == 'M')
4808 )
4809 {
4810 // save current mass count to get the connections and faces right.
4811 // their mass indices have to be increased by this number
4812 prev_mass_count = MechsimOptions.Data.mass_count;
4813
4814 line_num = 0;
4815
4816 switch ( fformat )
4817 {
4818 /* --- Old formats for backwards compatibility --- */
4819 case 1: case 2: case 3:
4820 {
4821 if (got_eof = fd->getline (line, MAX_STR).eof ())
4822 Warning(0, "\nError reading mechanics simulation data (wrong file format)");
4823
4824 Pos = line;
4825
4826 if (fformat>2) // file subtype 3 -> read start time
4827 Pos = Read_Float(fd, line, Pos, &MechsimOptions.Start_Time, &got_eof);
4828
4829 Pos = Read_Int(fd, line, Pos, &mass_count, &got_eof);
4830 Pos = Read_Int(fd, line, Pos, &connect_count, &got_eof);
4831 Pos = Read_Int(fd, line, Pos, &faces_count, &got_eof);
4832
4833 #if(MECHSIM_DEBUG == 1)
4834 Debug_Info("header %d %d %d\n", mass_count, connect_count, faces_count);
4835 //Debug_Info("line %d (%s)\n", line_num, line);
4836 #endif
4837
4838 while (!got_eof)
4839 {
4840 Send_ProgressUpdate(PROGRESS_MECHSIM_LOADFILE);
4841 Do_Cooperate(0);
4842
4843 line_num++;
4844
4845 #if(MECHSIM_DEBUG == 1)
4846 Debug_Info("line %d (%s)\n", line_num, Pos);
4847 //if (line_num==1) Debug_Info("\n(%s)\n", line);
4848 #endif
4849
4850 if (line_num <= mass_count) // ================ masses ===================
4851 {
4852 Pos = Read_Vector(fd, line, Pos, Posi, &got_eof);
4853 Pos = Read_Vector(fd, line, Pos, Vel, &got_eof);
4854 Pos = Read_Float(fd, line, Pos, &Mass, &got_eof);
4855 Pos = Read_Float(fd, line, Pos, &Rad, &got_eof);
4856 Pos = Read_Int(fd, line, Pos, &Flag, &got_eof);
4857
4858 if (fformat>1) // file contains group specifications
4859 {
4860 Pos = Read_Int(fd, line, Pos, &Grp, &got_eof);
4861 if (Grp<0)
4862 {
4863 Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
4864 Grp = 0;
4865 }
4866 }
4867 else Grp = 0;
4868
4869 if (fformat>2) // file subtype 3 -> read attachment index
4870 {
4871 Pos = Read_Int(fd, line, Pos, &Attach, &got_eof);
4872 }
4873 else Attach = -1;
4874
4875 if ( MechsimOptions.Data.mass_count >= MechsimOptions.Data.max_masses )
4876 {
4877 if (MechsimOptions.Data.max_masses >= INT_MAX/2)
4878 {
4879 Error("Too many masses in simulation data");
4880 }
4881 MechsimOptions.Data.max_masses *= 2;
4882 MechsimOptions.Data.Masses = (MECHSIM_MASS *)POV_REALLOC(MechsimOptions.Data.Masses, MechsimOptions.Data.max_masses*sizeof (MECHSIM_MASS), "mechanics simulation mass data block");
4883 }
4884
4885 Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Position, Posi);
4886 Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Velocity, Vel);
4887 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Mass = Mass;
4888 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Radius = Rad;
4889 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Flag = Flag;
4890 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Attach = Attach;
4891 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Force = -1;
4892
4893 if (Group==0) MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Grp; // no group given -> use those from file
4894 else MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Group;
4895
4896 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Index = MechsimOptions.Data.mass_count;
4897
4898 MechsimOptions.Data.mass_count++;
4899
4900 #if(MECHSIM_DEBUG == 1)
4901 Debug_Info("mass load\n");
4902 #endif
4903
4904 goodreads++;
4905
4906 }
4907 else if (line_num <= mass_count+connect_count) // ================ connections ================
4908 {
4909 Pos = Read_Int(fd, line, Pos, &Idx1, &got_eof);
4910 Pos = Read_Int(fd, line, Pos, &Idx2, &got_eof);
4911 Pos = Read_Float(fd, line, Pos, &Length, &got_eof);
4912 Pos = Read_Float(fd, line, Pos, &Stiff, &got_eof);
4913 Pos = Read_Float(fd, line, Pos, &Damp, &got_eof);
4914
4915 if (fformat>1) // file contains group specifications
4916 {
4917 Pos = Read_Int(fd, line, Pos, &Grp, &got_eof);
4918 if (Grp<0)
4919 {
4920 Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
4921 Grp = 0;
4922 }
4923 }
4924 else Grp = 0;
4925
4926 if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count))
4927 {
4928 Warning(0, "mechanics simulation data (line %d): connection index out of range, ignoring", line_num);
4929 continue;
4930 }
4931
4932 if ( MechsimOptions.Data.connect_count >= MechsimOptions.Data.max_connects )
4933 {
4934 if (MechsimOptions.Data.max_connects >= INT_MAX/2)
4935 {
4936 Error("Too many connections in simulation data");
4937 }
4938 MechsimOptions.Data.max_connects *= 2;
4939 MechsimOptions.Data.Connects = (MECHSIM_CONNECT *)POV_REALLOC(MechsimOptions.Data.Connects, MechsimOptions.Data.max_connects*sizeof (MECHSIM_CONNECT), "mechanics simulation connection data block");
4940 }
4941
4942 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx1 = Idx1+prev_mass_count;
4943 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx2 = Idx2+prev_mass_count;
4944
4945 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Length = Length;
4946 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Stiffness = Stiff;
4947 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Damping = Damp;
4948 if (Group==0) MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Grp; // no group given -> use those from file
4949 else MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Group;
4950
4951 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Index = MechsimOptions.Data.connect_count;
4952
4953 MechsimOptions.Data.connect_count++;
4954
4955 #if(MECHSIM_DEBUG == 1)
4956 Debug_Info("connect load %d %d %g %g %g\n", Idx1, Idx2, Length, Stiff, Damp);
4957 #endif
4958
4959 goodreads++;
4960
4961 }
4962 else if (line_num <= mass_count+connect_count+faces_count) // ================ faces ================
4963 {
4964 Pos = Read_Int(fd, line, Pos, &Idx1, &got_eof);
4965 Pos = Read_Int(fd, line, Pos, &Idx2, &got_eof);
4966 Pos = Read_Int(fd, line, Pos, &Idx3, &got_eof);
4967
4968 if (fformat>1) // file contains group specifications
4969 {
4970 Pos = Read_Int(fd, line, Pos, &Grp, &got_eof);
4971 if (Grp<0)
4972 {
4973 Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
4974 Grp = 0;
4975 }
4976 }
4977 else Grp = 0;
4978
4979 if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count) || (Idx3>=MechsimOptions.Data.mass_count))
4980 {
4981 Warning(0, "mechanics simulation data (line %d): face index out of range, ignoring", line_num);
4982 continue;
4983 }
4984
4985 if ( MechsimOptions.Data.faces_count >= MechsimOptions.Data.max_faces )
4986 {
4987 if (MechsimOptions.Data.max_faces >= INT_MAX/2)
4988 {
4989 Error("Too many faces in simulation data");
4990 }
4991 MechsimOptions.Data.max_faces *= 2;
4992 MechsimOptions.Data.Faces = (MECHSIM_FACE *)POV_REALLOC(MechsimOptions.Data.Faces, MechsimOptions.Data.max_faces*sizeof (MECHSIM_FACE), "mechanics simulation face data block");
4993 }
4994
4995 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx1 = Idx1+prev_mass_count;
4996 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx2 = Idx2+prev_mass_count;
4997 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx3 = Idx3+prev_mass_count;
4998 if (Group==0) MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Grp; // no group given -> use those from file
4999 else MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Group;
5000
5001 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Index = MechsimOptions.Data.faces_count;
5002
5003 MechsimOptions.Data.faces_count++;
5004
5005 goodreads++;
5006
5007 }
5008 else got_eof = true;
5009
5010 } /* end while-reading loop */
5011
5012 break;
5013 }
5014 /* --- New format - should be upwards compatible --- */
5015 case 4:
5016 {
5017 while (!(got_eof = fd->getline (line, MAX_STR).eof ()) && goodparse)
5018 {
5019 Send_ProgressUpdate(PROGRESS_MECHSIM_LOADFILE);
5020 Do_Cooperate(0);
5021
5022 line_num++;
5023
5024 switch ( line[0] )
5025 {
5026 case 'I': /* Info line with mass, connect and face count (and possibly others) */
5027 {
5028 sscanf(line, "I %d %d %d %d", &mass_count, &connect_count, &faces_count, &ve_count);
5029
5030 #if(MECHSIM_DEBUG == 1)
5031 Debug_Info("Info load: I %d %d %d %d\n", mass_count, connect_count, faces_count, ve_count);
5032 #endif
5033
5034 break;
5035 }
5036 case 'T': /* Timing line - Saved start time (and possibly other timing parameters) */
5037 {
5038 if (sscanf(line, "T %lf %lf %d", &Param1_DBL, &Param2_DBL, &Param1_Int) >= 3)
5039 {
5040 MechsimOptions.End_Time += Param1_DBL - MechsimOptions.Start_Time;
5041 MechsimOptions.Start_Time = Param1_DBL;
5042 MechsimOptions.Time = MechsimOptions.Start_Time;
5043 if (!MechsimOptions.Timing_Given)
5044 {
5045 MechsimOptions.Time_Step = Param2_DBL;
5046 MechsimOptions.Step_Count = Param1_Int;
5047 }
5048 /* always load current time step for adaptive method */
5049 else if (MechsimOptions.Method==3)
5050 {
5051 MechsimOptions.Time_Step = Param2_DBL;
5052 MechsimOptions.Step_Count = (int)((MechsimOptions.End_Time-MechsimOptions.Start_Time)/MechsimOptions.Time_Step);
5053 }
5054 MechsimOptions.Timing_Loaded = true;
5055 }
5056
5057 #if(MECHSIM_DEBUG == 1)
5058 Debug_Info("Timing load: I %g %g %d\n", Param1_DBL, Param2_DBL, Param1_Int);
5059 Debug_Info(" MechsimOptions.Method: %d\n", MechsimOptions.Method);
5060 Debug_Info(" MechsimOptions.Step_Count: %d\n", MechsimOptions.Step_Count);
5061 Debug_Info(" MechsimOptions.Time_Step: %g\n", MechsimOptions.Time_Step);
5062 Debug_Info(" MechsimOptions.Start_Time: %g\n", MechsimOptions.Start_Time);
5063 Debug_Info(" MechsimOptions.End_Time: %g\n", MechsimOptions.End_Time);
5064 #endif
5065
5066 break;
5067 }
5068 case 'M': /* Mass */
5069 {
5070 count = sscanf(line, "M %lf %lf %lf %lf %lf %lf %lf %lf %d %d %d %d",
5071 &Posi[X], &Posi[Y], &Posi[Z],
5072 &Vel[X], &Vel[Y], &Vel[Z],
5073 &Mass, &Rad, &Flag, &Grp, &Attach, &Force);
5074
5075 if (count >= 9)
5076 {
5077 if ( MechsimOptions.Data.mass_count >= MechsimOptions.Data.max_masses )
5078 {
5079 if (MechsimOptions.Data.max_masses >= INT_MAX/2)
5080 {
5081 Error("Too many masses in simulation data");
5082 }
5083 MechsimOptions.Data.max_masses *= 2;
5084 MechsimOptions.Data.Masses = (MECHSIM_MASS *)POV_REALLOC(MechsimOptions.Data.Masses, MechsimOptions.Data.max_masses*sizeof (MECHSIM_MASS), "mechanics simulation mass data block");
5085 }
5086
5087 if (count < 10) /* no group given */
5088 Grp = 0;
5089 else if (Grp<0)
5090 {
5091 Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
5092 Grp = 0;
5093 }
5094
5095 if (count < 10) /* no attach given */
5096 Attach = -1;
5097 if (count < 11) /* no force given */
5098 Force = -1;
5099
5100 Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Position, Posi);
5101 Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Velocity, Vel);
5102 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Mass = Mass;
5103 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Radius = Rad;
5104 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Flag = Flag;
5105 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Attach = Attach;
5106 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Force = Force;
5107
5108 if (Group==0) // no group given -> use those from file
5109 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Grp;
5110 else MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Group;
5111
5112 MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Index = MechsimOptions.Data.mass_count;
5113
5114 MechsimOptions.Data.mass_count++;
5115
5116 #if(MECHSIM_DEBUG == 1)
5117 Debug_Info("mass load\n");
5118 #endif
5119
5120 goodreads++;
5121 }
5122 else
5123 {
5124 Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
5125 }
5126
5127 break;
5128 }
5129 case 'C': /* Connect */
5130 {
5131 count = sscanf(line, "C %d %d %lf %lf %lf %d",
5132 &Idx1, &Idx2, &Length, &Stiff, &Damp, &Grp);
5133
5134 if (count >= 5)
5135 {
5136 if (count < 6) /* no group given */
5137 Grp = 0;
5138 else if (Grp<0)
5139 {
5140 Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
5141 Grp = 0;
5142 }
5143
5144 if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count))
5145 {
5146 Warning(0, "mechanics simulation data (line %d): connection index out of range, ignoring", line_num);
5147 continue;
5148 }
5149
5150 if ( MechsimOptions.Data.connect_count >= MechsimOptions.Data.max_connects )
5151 {
5152 if (MechsimOptions.Data.max_connects >= INT_MAX/2)
5153 {
5154 Error("Too many connections in simulation data");
5155 }
5156 MechsimOptions.Data.max_connects *= 2;
5157 MechsimOptions.Data.Connects = (MECHSIM_CONNECT *)POV_REALLOC(MechsimOptions.Data.Connects, MechsimOptions.Data.max_connects*sizeof (MECHSIM_CONNECT), "mechanics simulation connection data block");
5158 }
5159
5160 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx1 = Idx1+prev_mass_count;
5161 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx2 = Idx2+prev_mass_count;
5162
5163 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Length = Length;
5164 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Stiffness = Stiff;
5165 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Damping = Damp;
5166
5167 if (Group==0) // no group given -> use those from file
5168 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Grp;
5169 else MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Group;
5170
5171 MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Index = MechsimOptions.Data.connect_count;
5172
5173 MechsimOptions.Data.connect_count++;
5174
5175 #if(MECHSIM_DEBUG == 1)
5176 Debug_Info("connect load %d %d %g %g %g\n", Idx1, Idx2, Length, Stiff, Damp);
5177 #endif
5178
5179 goodreads++;
5180 }
5181 else
5182 {
5183 Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
5184 }
5185
5186 break;
5187 }
5188 case 'F': /* Face */
5189 {
5190 count = sscanf(line, "F %d %d %d %d", &Idx1, &Idx2, &Idx3, &Grp);
5191
5192 if (count >= 3)
5193 {
5194 if (count < 4) /* no group given */
5195 Grp = 0;
5196 else if (Grp<0)
5197 {
5198 Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
5199 Grp = 0;
5200 }
5201
5202 if ((Idx1>=MechsimOptions.Data.mass_count) ||
5203 (Idx2>=MechsimOptions.Data.mass_count) ||
5204 (Idx3>=MechsimOptions.Data.mass_count))
5205 {
5206 Warning(0, "mechanics simulation data (line %d): face index out of range, ignoring", line_num);
5207 continue;
5208 }
5209
5210 if ( MechsimOptions.Data.faces_count >= MechsimOptions.Data.max_faces )
5211 {
5212 if (MechsimOptions.Data.max_faces >= INT_MAX/2)
5213 {
5214 Error("Too many faces in simulation data");
5215 }
5216 MechsimOptions.Data.max_faces *= 2;
5217 MechsimOptions.Data.Faces = (MECHSIM_FACE *)POV_REALLOC(MechsimOptions.Data.Faces, MechsimOptions.Data.max_faces*sizeof (MECHSIM_FACE), "mechanics simulation face data block");
5218 }
5219
5220 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx1 = Idx1+prev_mass_count;
5221 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx2 = Idx2+prev_mass_count;
5222 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx3 = Idx3+prev_mass_count;
5223
5224 if (Group==0) // no group given -> use those from file
5225 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Grp;
5226 else MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Group;
5227
5228 MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Index = MechsimOptions.Data.faces_count;
5229
5230 MechsimOptions.Data.faces_count++;
5231
5232 #if(MECHSIM_DEBUG == 1)
5233 Debug_Info("face load %d %d %d\n", Idx1, Idx2, Idx3);
5234 #endif
5235
5236 goodreads++;
5237 }
5238 else
5239 {
5240 Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
5241 }
5242
5243 break;
5244 }
5245 case 'V': /* Viscoelastic */
5246 {
5247 count = sscanf(line, "V %d %d %lf %lf %d %d",
5248 &Idx1, &Idx2, &Length, &Stiff, &Acc, &ElCnt);
5249
5250 if ((count == 6) && (ElCnt >= 1))
5251 {
5252 if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count))
5253 {
5254 Warning(0, "mechanics simulation data (line %d): viscoelastic index out of range, ignoring", line_num);
5255 continue;
5256 }
5257
5258 if ( MechsimOptions.Data.ve_count >= MechsimOptions.Data.max_viscoelastics )
5259 {
5260 if (MechsimOptions.Data.max_viscoelastics >= INT_MAX/2)
5261 {
5262 Error("Too many viscoelastics in simulation data");
5263 }
5264 MechsimOptions.Data.max_viscoelastics *= 2;
5265 MechsimOptions.Data.Viscoelastics = (MECHSIM_VISCOELASTIC *)POV_REALLOC(MechsimOptions.Data.Viscoelastics, MechsimOptions.Data.max_viscoelastics*sizeof (MECHSIM_VISCOELASTIC), "mechanics simulation viscoelastic data block");
5266 }
5267
5268 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Idx1 = Idx1+prev_mass_count;
5269 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Idx2 = Idx2+prev_mass_count;
5270 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Length = Length;
5271 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Stiffness0 = Stiff;
5272 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Accuracy = Acc;
5273 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].element_count = ElCnt;
5274
5275 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element =
5276 (MECHSIM_VE_ELEMENT *)POV_MALLOC(ElCnt*sizeof (MECHSIM_VE_ELEMENT), "mechanics simulation viscoelastic data");
5277
5278 strncpy(line_param, line, MAX_STR);
5279 Pos = strtok(line_param, " "); // 'V'
5280 if (Pos != NULL) Pos = strtok(NULL, " "); // Index1
5281 if (Pos != NULL) Pos = strtok(NULL, " "); // Index2
5282 if (Pos != NULL) Pos = strtok(NULL, " "); // Length
5283 if (Pos != NULL) Pos = strtok(NULL, " "); // Stiffness0
5284 if (Pos != NULL) Pos = strtok(NULL, " "); // Accuracy
5285 if (Pos != NULL) Pos = strtok(NULL, " "); // Element count
5286 if (Pos == NULL)
5287 {
5288 Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
5289 parse_err = true;
5290 }
5291 else for (i=0; i<ElCnt; i++)
5292 {
5293 Pos = strtok(NULL, " ");
5294 if (Pos != NULL) MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Y = atof(Pos);
5295 else
5296 {
5297 Warning(0, "mechanics simulation data (line %d, 1): wrong syntax - skipping line", line_num);
5298 parse_err = true;
5299 break;
5300 }
5301 Pos = strtok(NULL, " ");
5302 if (Pos != NULL) MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Stiffness = atof(Pos);
5303 else
5304 {
5305 Warning(0, "mechanics simulation data (line %d, 2): wrong syntax - skipping line", line_num);
5306 parse_err = true;
5307 break;
5308 }
5309 Pos = strtok(NULL, " ");
5310 if (Pos != NULL) MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Damping = atof(Pos);
5311 else
5312 {
5313 Warning(0, "mechanics simulation data (line %d, 3): wrong syntax - skipping line", line_num);
5314 parse_err = true;
5315 break;
5316 }
5317
5318 #if(MECHSIM_DEBUG == 1)
5319 Debug_Info("ve element %g %g %g\n", MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Y, MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Stiffness, MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Damping );
5320 #endif
5321 }
5322
5323 if (parse_err)
5324 {
5325 parse_err = false;
5326 continue;
5327 }
5328
5329 MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Index = MechsimOptions.Data.ve_count;
5330
5331 MechsimOptions.Data.ve_count++;
5332
5333 #if(MECHSIM_DEBUG == 1)
5334 Debug_Info("ve load %d %d %g %g %d\n", Idx1, Idx2, Length, Stiff, ElCnt);
5335 #endif
5336
5337 goodreads++;
5338 }
5339 else
5340 {
5341 Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
5342 }
5343 break;
5344 }
5345
5346 default:
5347 {
5348 /* unknown leading character on line -> skip */
5349 }
5350
5351 } /* end switch */
5352 } /* end while-reading loop */
5353 break;
5354 }
5355 default: /* other file types */
5356 {
5357 Error("mechanics simulation data format %d not supported", fformat );
5358 return false;
5359 }
5360 }
5361 } /* end if "MSIM" */
5362
5363
5364 if ( !got_eof ) {
5365 Warning(0,"Error reading mechanics simulation data.");
5366 return false;
5367 }
5368 else
5369 {
5370 if ( goodreads > 0 )
5371 {
5372 Debug_Info("\nLoaded %d masses, %d connections, %d viscoelastics and %d faces\nfrom mechanics simulation data file %s.\n",
5373 MechsimOptions.Data.mass_count,
5374 MechsimOptions.Data.connect_count,
5375 MechsimOptions.Data.ve_count,
5376 MechsimOptions.Data.faces_count,
5377 MechsimOptions.Load_File_Name);
5378 }
5379 else
5380 {
5381 Warning(0,"Unable to read any values from mechanics simulation data file.");
5382 }
5383 return true;
5384 }
5385 }
5386 else return false;
5387
5388 }
5389
5390 /*****************************************************************************
5391 *
5392 * FUNCTION
5393 *
5394 * Mechsim_save_file
5395 *
5396 * INPUT
5397 * file descriptor handle of file (already opened).
5398 *
5399 * OUTPUT
5400 *
5401 * RETURNS 1 for success, 0 for failure.
5402 *
5403 * AUTHOUR
5404 *
5405 * Christoph Hormann
5406 *
5407 * DESCRIPTION
5408 *
5409 * Write mechanics simulation data to a file
5410 *
5411 * CHANGES
5412 *
5413 * --- Aug 2002 : Creation
5414 * Feb 2005 : new file type
5415 * Apr 2005 : Adding Force index
5416 *
5417 ******************************************************************************/
Mechsim_save_file(void)5418 bool Mechsim_save_file(void)
5419 {
5420 OStream *fd;
5421 int i, j;
5422 bool retval = false;
5423
5424 fd = New_OStream(MechsimOptions.Save_File_Name, POV_File_Unknown, false);
5425 if ( fd == NULL)
5426 {
5427 Error("Cannot save mechanics simulation data to file %s", MechsimOptions.Save_File_Name );
5428 retval = false;
5429 }
5430 else
5431 {
5432 Send_Progress("Writing Mechsim data to file", PROGRESS_MECHSIM_SAVEFILE);
5433
5434 fd->printf("MSIM, ");
5435
5436 fd->printf("%d,\n", MechsimOptions.Save_File_Type);
5437
5438 /* Info line */
5439 fd->printf("I %d %d %d\n",
5440 MechsimOptions.Data.mass_count,
5441 MechsimOptions.Data.connect_count,
5442 MechsimOptions.Data.faces_count,
5443 MechsimOptions.Data.ve_count);
5444
5445 /* Timing line */
5446 fd->printf("T %g %g %d\n",
5447 MechsimOptions.Time,
5448 MechsimOptions.Time_Step,
5449 MechsimOptions.Step_Count);
5450
5451 /* Masses */
5452 for (i=0; i<MechsimOptions.Data.mass_count; i++)
5453 {
5454 if ((i%100) == 0)
5455 {
5456 Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
5457 Do_Cooperate(0);
5458 }
5459
5460 fd->printf("M %.12f %.12f %.12f %.12f %.12f %.12f %g %g %d %d %d %d\n",
5461 MechsimOptions.Data.Masses[i].Position[X],
5462 MechsimOptions.Data.Masses[i].Position[Y],
5463 MechsimOptions.Data.Masses[i].Position[Z],
5464 MechsimOptions.Data.Masses[i].Velocity[X],
5465 MechsimOptions.Data.Masses[i].Velocity[Y],
5466 MechsimOptions.Data.Masses[i].Velocity[Z],
5467 MechsimOptions.Data.Masses[i].Mass,
5468 MechsimOptions.Data.Masses[i].Radius,
5469 MechsimOptions.Data.Masses[i].Flag,
5470 MechsimOptions.Data.Masses[i].Group,
5471 MechsimOptions.Data.Masses[i].Attach,
5472 MechsimOptions.Data.Masses[i].Force);
5473 }
5474
5475 /* Connections */
5476 for (i=0; i<MechsimOptions.Data.connect_count; i++)
5477 {
5478 if ((i%100) == 0)
5479 {
5480 Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
5481 Do_Cooperate(0);
5482 }
5483 fd->printf("C %d %d %g %g %g %d\n",
5484 MechsimOptions.Data.Connects[i].Idx1,
5485 MechsimOptions.Data.Connects[i].Idx2,
5486 MechsimOptions.Data.Connects[i].Length,
5487 MechsimOptions.Data.Connects[i].Stiffness,
5488 MechsimOptions.Data.Connects[i].Damping,
5489 MechsimOptions.Data.Connects[i].Group);
5490 }
5491
5492 /* Faces */
5493 for (i=0; i<MechsimOptions.Data.faces_count; i++)
5494 {
5495 if ((i%100) == 0)
5496 {
5497 Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
5498 Do_Cooperate(0);
5499 }
5500 fd->printf("F %d %d %d %d\n",
5501 MechsimOptions.Data.Faces[i].Idx1,
5502 MechsimOptions.Data.Faces[i].Idx2,
5503 MechsimOptions.Data.Faces[i].Idx3,
5504 MechsimOptions.Data.Faces[i].Group);
5505 }
5506
5507 /* Viscoelastics */
5508 for (i=0; i<MechsimOptions.Data.ve_count; i++)
5509 {
5510 if ((i%100) == 0)
5511 {
5512 Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
5513 Do_Cooperate(0);
5514 }
5515 fd->printf("V %d %d %g %g %d %d",
5516 MechsimOptions.Data.Viscoelastics[i].Idx1,
5517 MechsimOptions.Data.Viscoelastics[i].Idx2,
5518 MechsimOptions.Data.Viscoelastics[i].Length,
5519 MechsimOptions.Data.Viscoelastics[i].Stiffness0,
5520 MechsimOptions.Data.Viscoelastics[i].Accuracy,
5521 MechsimOptions.Data.Viscoelastics[i].element_count);
5522
5523 for (j=0; j<MechsimOptions.Data.Viscoelastics[i].element_count; j++)
5524 {
5525 fd->printf(" %g %g %g",
5526 MechsimOptions.Data.Viscoelastics[i].Element[j].Y,
5527 MechsimOptions.Data.Viscoelastics[i].Element[j].Stiffness,
5528 MechsimOptions.Data.Viscoelastics[i].Element[j].Damping);
5529
5530 }
5531 fd->printf("\n");
5532 }
5533
5534 fd->printf("\n");
5535
5536 delete fd;
5537
5538 Debug_Info("Saved simulation data to file %s\n", MechsimOptions.Save_File_Name);
5539
5540 retval = true;
5541 }
5542
5543 POV_FREE(MechsimOptions.Save_File_Name);
5544 MechsimOptions.Save_File_Name = NULL;
5545
5546 return retval;
5547 }
5548
5549
5550
5551 // ========================================================================
5552
Read_Vector(IStream * fd,char * str,char * buffer,VECTOR Vect,bool * got_eof)5553 char *Read_Vector(IStream *fd, char *str, char *buffer, VECTOR Vect, bool *got_eof)
5554 {
5555 char *Pos;
5556 char *End;
5557 char *ptr;
5558 char str_nbr[MAX_STR];
5559
5560 if (*got_eof) return NULL;
5561 if (buffer == NULL)
5562 {
5563 *got_eof = true;
5564 return NULL;
5565 }
5566
5567 //Debug_Info("Read_Vector() %s\n", buffer);
5568
5569 // search vector start
5570 Pos=strchr(buffer, '<');
5571 if (Pos == NULL)
5572 {
5573 // read next line when necessary
5574 *got_eof = fd->getline (str, MAX_STR).eof ();
5575
5576 str[MAX_STR-1] = '\0';
5577 buffer = str;
5578
5579 Pos = strchr(buffer, '\n'); // remove line break
5580 if (Pos != NULL) Pos[0] = '\0';
5581 Pos = strchr(buffer, '\r'); // remove line break
5582 if (Pos != NULL) Pos[0] = '\0';
5583
5584 Pos=strchr(buffer, '<');
5585 if (Pos == NULL)
5586 {
5587 Warning(0,"Warning: strange format of mechanics simulation data (v2)");
5588
5589 Vect[X] = 0.0;
5590 Vect[Y] = 0.0;
5591 Vect[Z] = 0.0;
5592 return buffer;
5593 }
5594 }
5595
5596 if (*got_eof) return NULL;
5597
5598 // read vector content
5599 Pos++;
5600 strncpy(str_nbr, Pos, MAX_STR);
5601
5602 Pos = strtok(str_nbr, ", ");
5603 if (Pos != NULL)
5604 {
5605 Vect[X] = strtod(Pos, &End);
5606 Pos = strtok(NULL, ", ");
5607 if (Pos != NULL)
5608 {
5609 Vect[Y] = strtod(Pos, &End);
5610 Pos = strtok(NULL, ", ");
5611 if (Pos != NULL)
5612 {
5613 Vect[Z] = strtod(Pos, &End);
5614 }
5615 }
5616 }
5617 else
5618 {
5619 Warning(0,"Warning: strange format of mechanics simulation data (v1)");
5620
5621 Vect[X] = 0.0;
5622 Vect[Y] = 0.0;
5623 Vect[Z] = 0.0;
5624 return buffer;
5625 }
5626
5627 Pos = strchr(buffer, '>'); // search vector end
5628 Pos++;
5629 ptr = strchr(Pos, ',');
5630 if (ptr == NULL) ptr=strchr(Pos, ' ');
5631 if (ptr == NULL) ptr=strchr(Pos, '\t');
5632
5633 if (ptr == NULL) // no delimiter
5634 return Pos;
5635
5636 ptr++;
5637
5638 return ptr;
5639
5640 }
5641
5642 // ========================================================================
5643
Read_Float(IStream * fd,char * str,char * buffer,double * Value,bool * got_eof)5644 char *Read_Float(IStream *fd, char *str, char *buffer, double *Value, bool *got_eof)
5645 {
5646 char *Pos;
5647 char *End;
5648
5649 if (*got_eof) return NULL;
5650 if (buffer == NULL)
5651 {
5652 *got_eof = true;
5653 return NULL;
5654 }
5655
5656 //Debug_Info("Read_Float() %s\n", buffer);
5657
5658 Pos=strchr(buffer, ',');
5659 if (Pos == NULL) Pos=strchr(buffer, ' ');
5660 if (Pos == NULL) Pos=strchr(buffer, '\t');
5661 if ((Pos == NULL) && (buffer[0] == '\0'))
5662 {
5663 // read next line when necessary
5664 *got_eof = fd->getline (str, MAX_STR).eof ();
5665
5666 str[MAX_STR-1] = '\0';
5667 buffer = str;
5668
5669 Pos = strchr(buffer, '\n'); // remove line break
5670 if (Pos != NULL) Pos[0] = '\0';
5671 Pos = strchr(buffer, '\r'); // remove line break
5672 if (Pos != NULL) Pos[0] = '\0';
5673
5674 Pos=strchr(buffer, ',');
5675 if (Pos == NULL) Pos=strchr(buffer, ' ');
5676 if (Pos == NULL) Pos=strchr(buffer, '\t');
5677 if (Pos == NULL)
5678 {
5679 Warning(0,"Warning: strange format of mechanics simulation data (f)");
5680 return buffer;
5681 }
5682 }
5683
5684 if (*got_eof) return NULL;
5685
5686 Pos[0] = '\0'; // cut comma
5687
5688 *Value = strtod(buffer, &End);
5689
5690 Pos++;
5691
5692 return Pos;
5693
5694 }
5695
5696 // ========================================================================
5697
5698
Read_Int(IStream * fd,char * str,char * buffer,int * Value,bool * got_eof)5699 char *Read_Int(IStream *fd, char *str, char *buffer, int *Value, bool *got_eof)
5700 {
5701 char *Pos;
5702 char *End;
5703
5704 if (*got_eof) return NULL;
5705 if (buffer == NULL)
5706 {
5707 *got_eof = true;
5708 return NULL;
5709 }
5710
5711 //Debug_Info("Read_Int() %s\n", buffer);
5712
5713 Pos=strchr(buffer, ',');
5714 if (Pos == NULL) Pos=strchr(buffer, ' ');
5715 if (Pos == NULL) Pos=strchr(buffer, '\t');
5716 if ((Pos == NULL) && (buffer[0] == '\0'))
5717 {
5718 // read next line when necessary
5719 *got_eof = fd->getline (str, MAX_STR).eof ();
5720
5721 str[MAX_STR-1] = '\0';
5722 buffer = str;
5723
5724 Pos = strchr(buffer, '\n'); // remove line break
5725 if (Pos != NULL) Pos[0] = '\0';
5726 Pos = strchr(buffer, '\r'); // remove line break
5727 if (Pos != NULL) Pos[0] = '\0';
5728
5729 Pos=strchr(buffer, ',');
5730 if (Pos == NULL) Pos=strchr(buffer, ' ');
5731 if (Pos == NULL) Pos=strchr(buffer, '\t');
5732 if (Pos == NULL)
5733 {
5734 Warning(0,"Warning: strange format of mechanics simulation data (i)");
5735 return buffer;
5736 }
5737 }
5738
5739 if (*got_eof) return NULL;
5740
5741 Pos[0] = '\0'; // cut comma
5742
5743 *Value = strtol(buffer, &End, 0);
5744
5745 Pos++;
5746
5747 return Pos;
5748
5749 }
5750
5751 /*****************************************************************************
5752 *
5753 * FUNCTION
5754 *
5755 * calc_faces_bounding_box
5756 *
5757 * INPUT
5758 * Bounding_Data all bounding data.
5759 *
5760 * OUTPUT
5761 *
5762 * RETURNS
5763 *
5764 * AUTHOUR
5765 *
5766 * Daniel Jungmann
5767 *
5768 * DESCRIPTION
5769 *
5770 * Calculates the bounding boxes for all faces and the unit size of the grid for faces.
5771 *
5772 * CHANGES
5773 *
5774 * --- Mar 2004 : Creation
5775 *
5776 ******************************************************************************/
calc_faces_bounding_box(VECTOR * Pos_Array,MECHSIM_BOUNDING_DATA * Bounding_Data)5777 static void calc_faces_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
5778 {
5779 SNGL_VECT Unit;
5780 SNGL_VECT t_max, t_min, tmp;
5781 int Idx1, Idx2, Idx3, i;
5782
5783 Make_Vector(tmp, 0.0, 0.0, 0.0);
5784 for (i = 0; i < MechsimOptions.Data.faces_count; i++)
5785 {
5786 Idx1 = MechsimOptions.Data.Faces[i].Idx1;
5787 Idx2 = MechsimOptions.Data.Faces[i].Idx2;
5788 Idx3 = MechsimOptions.Data.Faces[i].Idx3;
5789 t_max[X] = max(Pos_Array[Idx1][X], max(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
5790 t_max[Y] = max(Pos_Array[Idx1][Y], max(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
5791 t_max[Z] = max(Pos_Array[Idx1][Z], max(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));
5792 t_min[X] = min(Pos_Array[Idx1][X], min(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
5793 t_min[Y] = min(Pos_Array[Idx1][Y], min(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
5794 t_min[Z] = min(Pos_Array[Idx1][Z], min(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));
5795
5796 Make_BBox_from_min_max(Bounding_Data->Faces_BBox_Array[i], t_min, t_max);
5797
5798 VSubEq(t_max, t_min);
5799 VAddEq(tmp, t_max);
5800 }
5801 VScale(Unit, tmp, 1.0/MechsimOptions.Data.faces_count);
5802 Assign_Vector(Bounding_Data->Unit, Unit);
5803 }
5804
5805 /*****************************************************************************
5806 *
5807 * FUNCTION
5808 *
5809 * intersect_sphere_box
5810 *
5811 * INPUT
5812 * Box bounding box.
5813 * Center center of sphere.
5814 * Radius radius of sphere.
5815 *
5816 * OUTPUT
5817 *
5818 * RETURNS true if sphere intersects box else false.
5819 *
5820 * AUTHOUR
5821 *
5822 * Daniel Jungmann
5823 *
5824 * DESCRIPTION
5825 *
5826 * Test if a sphere intersects a box.
5827 *
5828 * CHANGES
5829 *
5830 * --- Mar 2004 : Creation
5831 *
5832 ******************************************************************************/
intersect_sphere_box(BBOX BBox,VECTOR Center,DBL Radius)5833 static bool intersect_sphere_box(BBOX BBox, VECTOR Center, DBL Radius)
5834 {
5835 SNGL d;
5836 SNGL_VECT tmp, s, SNGL_Center;
5837
5838 VScale(tmp, BBox.Lengths, 0.5);
5839 VAdd(s, BBox.Lower_Left, tmp);
5840 Assign_Vector(SNGL_Center, Center);
5841 VSub(s, SNGL_Center, s);
5842 s[X] = fabs(s[X]);
5843 s[Y] = fabs(s[Y]);
5844 s[Z] = fabs(s[Z]);
5845 VSubEq(s, tmp);
5846 s[X] = max(s[X], 0.0f);
5847 s[Y] = max(s[Y], 0.0f);
5848 s[Z] = max(s[Z], 0.0f);
5849
5850 d = s[X]*s[X]+s[Y]*s[Y]+s[Z]*s[Z];
5851
5852 return d <= Radius*Radius;
5853 }
5854
5855 /*****************************************************************************
5856 *
5857 * FUNCTION
5858 *
5859 * calc_faces_bounding_hashtable
5860 *
5861 * INPUT
5862 * Bounding_Data all bounding data.
5863 *
5864 * OUTPUT
5865 *
5866 * RETURNS
5867 *
5868 * AUTHOUR
5869 *
5870 * Daniel Jungmann
5871 *
5872 * DESCRIPTION
5873 *
5874 * Calculates the bounding hashtable for all faces.
5875 *
5876 * CHANGES
5877 *
5878 * --- Mar 2004 : Creation
5879 *
5880 ******************************************************************************/
calc_faces_bounding_hashtable(MECHSIM_BOUNDING_DATA * Bounding_Data)5881 static void calc_faces_bounding_hashtable(MECHSIM_BOUNDING_DATA* Bounding_Data)
5882 {
5883 long int jx_min, jy_min, jz_min;
5884 long int jx_max, jy_max, jz_max;
5885 long int jx, jy, jz;
5886 int i;
5887 unsigned int hash;
5888
5889 for (i = 0; i < MechsimOptions.Data.faces_count; i++)
5890 {
5891 jx_min = (long int)floor(Bounding_Data->Faces_BBox_Array[i].Lower_Left[X]/
5892 Bounding_Data->Unit[X]);
5893 jy_min = (long int)floor(Bounding_Data->Faces_BBox_Array[i].Lower_Left[Y]/
5894 Bounding_Data->Unit[Y]);
5895 jz_min = (long int)floor(Bounding_Data->Faces_BBox_Array[i].Lower_Left[Z]/
5896 Bounding_Data->Unit[Z]);
5897
5898 jx_max = (long int)floor((Bounding_Data->Faces_BBox_Array[i].Lower_Left[X]+
5899 Bounding_Data->Faces_BBox_Array[i].Lengths[X])/Bounding_Data->Unit[X]);
5900 jy_max = (long int)floor((Bounding_Data->Faces_BBox_Array[i].Lower_Left[Y]+
5901 Bounding_Data->Faces_BBox_Array[i].Lengths[Y])/Bounding_Data->Unit[Y]);
5902 jz_max = (long int)floor((Bounding_Data->Faces_BBox_Array[i].Lower_Left[Z]+
5903 Bounding_Data->Faces_BBox_Array[i].Lengths[Z])/Bounding_Data->Unit[Z]);
5904
5905 for (jx = jx_min; jx <= jx_max; jx++)
5906 for (jy = jy_min; jy <= jy_max; jy++)
5907 for (jz = jz_min; jz <= jz_max; jz++)
5908 {
5909 hash = bounding_hash_func(jx, jy, jz, Bounding_Data->bounding_hashtable_size);
5910 insert_index_bounding_hastable(Bounding_Data->bounding_hashtable,
5911 hash, i, Bounding_Data->time_stamp);
5912 }
5913 }
5914 }
5915
5916 /*****************************************************************************
5917 *
5918 * FUNCTION
5919 *
5920 * insert_index_bounding_hastable
5921 *
5922 * INPUT
5923 * bounding_hashtable bounding hashtable.
5924 * hash hash of cell.
5925 * index index of triangle.
5926 * time_stamp time stamp for faster updating of the hashtable.
5927 *
5928 * OUTPUT
5929 *
5930 * RETURNS
5931 *
5932 * AUTHOUR
5933 *
5934 * Daniel Jungmann
5935 *
5936 * DESCRIPTION
5937 *
5938 * Insert an index into the bounding hashtable at position hash.
5939 *
5940 * CHANGES
5941 *
5942 * --- Mar 2004 : Creation
5943 *
5944 ******************************************************************************/
insert_index_bounding_hastable(BOUNDING_HASHTABLE_ITEM * bounding_hashtable,unsigned int hash,int index,int time_stamp)5945 static void insert_index_bounding_hastable(BOUNDING_HASHTABLE_ITEM* bounding_hashtable, unsigned int hash, int index, int time_stamp)
5946 {
5947 int size, used;
5948 if ((bounding_hashtable[hash].size == 0) || (bounding_hashtable[hash].index == NULL))
5949 {
5950 size = 4;
5951 used = 0;
5952 bounding_hashtable[hash].index = (int *)POV_MALLOC(size*sizeof(int), "mechsim bounding hashtable index's");
5953 bounding_hashtable[hash].size = size;
5954 }
5955 else
5956 {
5957 size = bounding_hashtable[hash].size;
5958 if (bounding_hashtable[hash].time_stamp < time_stamp) used = 0;
5959 else
5960 {
5961 used = bounding_hashtable[hash].used;
5962 if (size < (used+1))
5963 {
5964 size *= 2;
5965 bounding_hashtable[hash].index = (int *)POV_REALLOC(bounding_hashtable[hash].index, size*sizeof(int),
5966 "mechsim bounding hashtable index's");
5967 bounding_hashtable[hash].size = size;
5968 }
5969 }
5970 }
5971 bounding_hashtable[hash].index[used] = index;
5972 bounding_hashtable[hash].time_stamp = time_stamp;
5973 bounding_hashtable[hash].used = used+1;
5974 }
5975
5976 /*****************************************************************************
5977 *
5978 * FUNCTION
5979 *
5980 * intersect_sphere_bounding_hashtable
5981 *
5982 * INPUT
5983 * Center center of sphere.
5984 * Radius radius of sphere.
5985 * Bounding_Data all bounding data.
5986 *
5987 * OUTPUT
5988 *
5989 * RETURNS
5990 *
5991 * AUTHOUR
5992 *
5993 * Daniel Jungmann
5994 *
5995 * DESCRIPTION
5996 *
5997 * Test if a sphere intersects a bounding hashtable.
5998 *
5999 * CHANGES
6000 *
6001 * --- Mar 2004 : Creation
6002 *
6003 ******************************************************************************/
intersect_sphere_bounding_hashtable(VECTOR Center,DBL Radius,MECHSIM_BOUNDING_DATA * Bounding_Data)6004 static void intersect_sphere_bounding_hashtable(VECTOR Center, DBL Radius, MECHSIM_BOUNDING_DATA* Bounding_Data)
6005 {
6006 BBOX BBox;
6007 long int jx_min, jy_min, jz_min;
6008 long int jx_max, jy_max, jz_max;
6009 long int jx, jy, jz;
6010 unsigned int hash;
6011 int i;
6012 int idx;
6013
6014 jx_min = (long int)floor((Center[X]-Radius)/Bounding_Data->Unit[X]);
6015 jy_min = (long int)floor((Center[Y]-Radius)/Bounding_Data->Unit[Y]);
6016 jz_min = (long int)floor((Center[Z]-Radius)/Bounding_Data->Unit[Z]);
6017
6018 jx_max = (long int)floor((Center[X]+Radius)/Bounding_Data->Unit[X]);
6019 jy_max = (long int)floor((Center[Y]+Radius)/Bounding_Data->Unit[Y]);
6020 jz_max = (long int)floor((Center[Z]+Radius)/Bounding_Data->Unit[Z]);
6021
6022 /* Kein Index wurde Kopiert, deswegen alle auf Null setzen.*/
6023 memset(Bounding_Data->bitfield, 0, sizeof(unsigned int)*Bounding_Data->bitfield_size);
6024
6025 idx = 0;
6026 BBox.Lengths[X] = Bounding_Data->Unit[X];
6027 BBox.Lengths[Y] = Bounding_Data->Unit[Y];
6028 BBox.Lengths[Z] = Bounding_Data->Unit[Z];
6029 BBox.Lower_Left[X] = jx_min*Bounding_Data->Unit[X];
6030 for (jx = jx_min; jx <= jx_max; jx++)
6031 {
6032 BBox.Lower_Left[Y] = jy_min*Bounding_Data->Unit[Y];
6033 for (jy = jy_min; jy <= jy_max; jy++)
6034 {
6035 BBox.Lower_Left[Z] = jz_min*Bounding_Data->Unit[Z];
6036 for (jz = jz_min; jz <= jz_max; jz++)
6037 {
6038 /* We test if the sphere intersects this bounding box */
6039 if (intersect_sphere_box(BBox, Center, Radius))
6040 {
6041 hash = bounding_hash_func(jx, jy, jz, Bounding_Data->bounding_hashtable_size);
6042 /* Ist der Eintrag überhaupt aktuell? */
6043 if (Bounding_Data->bounding_hashtable[hash].time_stamp == Bounding_Data->time_stamp)
6044 {
6045 /* Brauchen wir vieleicht mehr Speicher? */
6046 if (Bounding_Data->intersect_list_size < (idx + Bounding_Data->bounding_hashtable[hash].used))
6047 {
6048 Bounding_Data->intersect_list_size += Bounding_Data->bounding_hashtable[hash].used;
6049 Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list,
6050 Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding hashtable intersect list");
6051 }
6052 /* Kopieren der Indizies. */
6053 for (i = 0; i < Bounding_Data->bounding_hashtable[hash].used; i++)
6054 {
6055 /* Nur Kopieren, was nicht schon da ist. */
6056 if (!testbit(Bounding_Data->bitfield, Bounding_Data->bounding_hashtable[hash].index[i]))
6057 {
6058 Bounding_Data->intersect_list[idx] = Bounding_Data->bounding_hashtable[hash].index[i];
6059 idx++;
6060 /* Dieser Index ist nun Kopiert, deswegen wird das entsprechende Bit gesetzt. */
6061 setbit(Bounding_Data->bitfield, Bounding_Data->bounding_hashtable[hash].index[i]);
6062 }
6063 }
6064 }
6065 }
6066 BBox.Lower_Left[Z] += Bounding_Data->Unit[Z];
6067 }
6068 BBox.Lower_Left[Y] += Bounding_Data->Unit[Y];
6069 }
6070 BBox.Lower_Left[X] += Bounding_Data->Unit[X];
6071 }
6072
6073 Bounding_Data->intersect_list_index = idx;
6074 }
6075
6076 /*****************************************************************************
6077 *
6078 * FUNCTION
6079 *
6080 * calc_face_groups_bounding_box
6081 *
6082 * INPUT
6083 * Pos_Array array of masses positions.
6084 * Bounding_Data all bounding data.
6085 *
6086 * OUTPUT
6087 *
6088 * RETURNS
6089 *
6090 * AUTHOUR
6091 *
6092 * Daniel Jungmann
6093 *
6094 * DESCRIPTION
6095 *
6096 * Berechnet die Bounding-Boxen für alle Gruppen. Dabei werden nur die Seiten berücksichtigt.
6097 *
6098 * CHANGES
6099 *
6100 * --- April 2004 : Creation
6101 *
6102 ******************************************************************************/
calc_face_groups_bounding_box(VECTOR * Pos_Array,MECHSIM_BOUNDING_DATA * Bounding_Data)6103 static void calc_face_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
6104 {
6105 VECTOR t_max, t_min, g_max, g_min;
6106 int Idx1, Idx2, Idx3, i, j, Index;
6107
6108 for (i = 0; i < MechsimOptions.Data.group_count; i++)
6109 {
6110 Make_Vector(g_max, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
6111 /* Die Gruppen enthält keine Seiten, also kann die Bounding-Box die Größe Null haben und möglichst weit weg liegen. */
6112 if (MechsimOptions.Data.Groups[i].faces_count == 0)
6113 {
6114 Make_Vector(g_min, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
6115 Make_BBox_from_min_max(Bounding_Data->Face_Groups_BBox_Array[i], g_min, g_max);
6116 }
6117 else
6118 {
6119 Make_Vector(g_min, BOUND_HUGE, BOUND_HUGE, BOUND_HUGE);
6120
6121 for (j = 0; j < MechsimOptions.Data.Groups[i].faces_count; j++)
6122 {
6123 Index = MechsimOptions.Data.Groups[i].Faces[j]->Index;
6124 Idx1 = MechsimOptions.Data.Groups[i].Faces[j]->Idx1;
6125 Idx2 = MechsimOptions.Data.Groups[i].Faces[j]->Idx2;
6126 Idx3 = MechsimOptions.Data.Groups[i].Faces[j]->Idx3;
6127 t_max[X] = max(Pos_Array[Idx1][X], max(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
6128 t_max[Y] = max(Pos_Array[Idx1][Y], max(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
6129 t_max[Z] = max(Pos_Array[Idx1][Z], max(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));
6130 t_min[X] = min(Pos_Array[Idx1][X], min(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
6131 t_min[Y] = min(Pos_Array[Idx1][Y], min(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
6132 t_min[Z] = min(Pos_Array[Idx1][Z], min(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));
6133
6134 /* Bounding-Box der Seite. */
6135 Make_BBox_from_min_max(Bounding_Data->Faces_BBox_Array[Index], t_min, t_max);
6136
6137 g_max[X] = max(g_max[X], t_max[X]);
6138 g_max[Y] = max(g_max[Y], t_max[Y]);
6139 g_max[Z] = max(g_max[Z], t_max[Z]);
6140 g_min[X] = min(g_min[X], t_min[X]);
6141 g_min[Y] = min(g_min[Y], t_min[Y]);
6142 g_min[Z] = min(g_min[Z], t_min[Z]);
6143 }
6144 /* Bounding-Box der Gruppe. */
6145 Make_BBox_from_min_max(Bounding_Data->Face_Groups_BBox_Array[i], g_min, g_max);
6146 }
6147 }
6148 }
6149
6150 /*****************************************************************************
6151 *
6152 * FUNCTION
6153 *
6154 * intersect_box_box
6155 *
6156 * INPUT
6157 * BBox_1 box number one.
6158 * BBox_2 box number two.
6159 *
6160 * OUTPUT
6161 *
6162 * RETURNS true if box one intersects box two else false.
6163 *
6164 * AUTHOUR
6165 *
6166 * Daniel Jungmann
6167 *
6168 * DESCRIPTION
6169 *
6170 * Gibt true zurück, wenn die zwei Quader sich berühre oder eine Schnittmenge
6171 * besitzen, die nicht leer ist.
6172 *
6173 * CHANGES
6174 *
6175 * --- Mar 2004 : Creation
6176 *
6177 ******************************************************************************/
6178
intersect_box_box(BBOX BBox_1,BBOX BBox_2)6179 static bool intersect_box_box(BBOX BBox_1, BBOX BBox_2)
6180 {
6181 SNGL_VECT tmp1, tmp2, tmp3;
6182 VAdd(tmp1, BBox_1.Lower_Left, BBox_1.Lengths);
6183 VAdd(tmp2, BBox_2.Lower_Left, BBox_2.Lengths);
6184 tmp3[X] = max(BBox_1.Lower_Left[X], BBox_2.Lower_Left[X]);
6185 tmp3[Y] = max(BBox_1.Lower_Left[Y], BBox_2.Lower_Left[Y]);
6186 tmp3[Z] = max(BBox_1.Lower_Left[Z], BBox_2.Lower_Left[Z]);
6187 tmp1[X] = min(tmp1[X], tmp2[X]);
6188 tmp1[Y] = min(tmp1[Y], tmp2[Y]);
6189 tmp1[Z] = min(tmp1[Z], tmp2[Z]);
6190 return ((tmp3[X] <= tmp1[X]) && (tmp3[Y] <= tmp1[Y]) && (tmp3[Z] <= tmp1[Z]));
6191 }
6192
6193 /*****************************************************************************
6194 * Berechnet welche Seiten in der Schnittmenge liegen. Nur die Seiten, die
6195 * tatsächlich geschnitten werden, müssen später überprüft werde.
6196 ******************************************************************************/
calc_face_group_intersection(BBOX intersection_Box,int Grp,MECHSIM_BOUNDING_DATA * Bounding_Data)6197 static void calc_face_group_intersection(BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data)
6198 {
6199 int i, Idx, Index;
6200
6201 if (Bounding_Data->intersect_list_size < MechsimOptions.Data.Groups[Grp].faces_count)
6202 {
6203 Bounding_Data->intersect_list_size = MechsimOptions.Data.Groups[Grp].faces_count;
6204 Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list,
6205 Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding box intersect list");
6206 }
6207 Index = 0;
6208 for (i = 0; i < MechsimOptions.Data.Groups[Grp].faces_count; i++)
6209 {
6210 Idx = MechsimOptions.Data.Groups[Grp].Faces[i]->Index;
6211 if (intersect_box_box(intersection_Box, Bounding_Data->Faces_BBox_Array[Idx]))
6212 {
6213 Bounding_Data->intersect_list[Index] = Idx;
6214 Index++;
6215 }
6216 }
6217 Bounding_Data->intersect_list_index = Index;
6218 }
6219
6220 /*****************************************************************************
6221 *
6222 * FUNCTION
6223 *
6224 * init_bounding
6225 *
6226 * INPUT
6227 *
6228 * OUTPUT
6229 * Bounding_Data all bounding data.
6230 *
6231 * RETURNS
6232 *
6233 * AUTHOUR
6234 *
6235 * Daniel Jungmann
6236 *
6237 * DESCRIPTION
6238 *
6239 * Initialisiert alle Boundingdaten.
6240 *
6241 * CHANGES
6242 *
6243 * --- Mar 2004 : Creation
6244 *
6245 ******************************************************************************/
6246
init_bounding(void)6247 static MECHSIM_BOUNDING_DATA* init_bounding(void)
6248 {
6249 MECHSIM_BOUNDING_DATA* Bounding_Data = NULL;
6250
6251 /* Initialization of bounding */
6252 if (MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_NO)
6253 {
6254 Bounding_Data = (MECHSIM_BOUNDING_DATA *)POV_MALLOC(sizeof(MECHSIM_BOUNDING_DATA), "mechsim bounding data");
6255 memset(Bounding_Data, 0, sizeof(MECHSIM_BOUNDING_DATA));
6256
6257 Bounding_Data->intersect_list_size = 8;
6258 Bounding_Data->intersect_list_index = 0;
6259
6260 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
6261 {
6262 Bounding_Data->bounding_hashtable_size = MechsimOptions.Data.faces_count;
6263 Bounding_Data->bounding_hashtable = (BOUNDING_HASHTABLE_ITEM*)POV_MALLOC(Bounding_Data->bounding_hashtable_size
6264 *sizeof(BOUNDING_HASHTABLE_ITEM), "mechsim faces bounding hashtable");
6265 memset(Bounding_Data->bounding_hashtable, 0, Bounding_Data->bounding_hashtable_size*sizeof(BOUNDING_HASHTABLE_ITEM));
6266
6267 Bounding_Data->time_stamp = 1;
6268
6269 Bounding_Data->bitfield_size = MechsimOptions.Data.faces_count/BITSPERINT+1;
6270 Bounding_Data->bitfield = (unsigned int *)POV_MALLOC(Bounding_Data->bitfield_size*sizeof(unsigned int),
6271 "mechsim bounding hashtable bitfield");
6272
6273 Bounding_Data->intersect_list = (int *)POV_MALLOC(Bounding_Data->intersect_list_size*sizeof(int),
6274 "mechsim bounding hashtable intersect list");
6275
6276 }
6277 else
6278 {
6279 Bounding_Data->intersect_list = (int *)POV_MALLOC(Bounding_Data->intersect_list_size*sizeof(int),
6280 "mechsim bounding box intersect list");
6281 }
6282
6283 Bounding_Data->Faces_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_faces*sizeof(BBOX),
6284 "mechsim faces bounding boxes");
6285 Bounding_Data->Face_Groups_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof(BBOX),
6286 "mechsim face groups bounding boxes");
6287 Bounding_Data->Mass_Groups_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof(BBOX),
6288 "mechsim mass groups bounding boxes");
6289 Bounding_Data->Connect_Groups_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof(BBOX),
6290 "mechsim connections groups bounding boxes");
6291 Bounding_Data->Connects_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_connects*sizeof(BBOX),
6292 "mechsim connections bounding boxes");
6293 }
6294 return Bounding_Data;
6295 }
6296
6297 /*****************************************************************************
6298 *
6299 * FUNCTION
6300 *
6301 * free_bounding
6302 *
6303 * INPUT
6304 * Bounding_Data all bounding data.
6305 *
6306 * OUTPUT
6307 *
6308 * RETURNS
6309 *
6310 * AUTHOUR
6311 *
6312 * Daniel Jungmann
6313 *
6314 * DESCRIPTION
6315 *
6316 * Gibt alle Boundingdaten frei.
6317 *
6318 * CHANGES
6319 *
6320 * --- Mar 2004 : Creation
6321 *
6322 ******************************************************************************/
6323
free_bounding(MECHSIM_BOUNDING_DATA * Bounding_Data)6324 static void free_bounding(MECHSIM_BOUNDING_DATA* Bounding_Data)
6325 {
6326 int i;
6327
6328 /* Deinitialization of bounding */
6329 if (MechsimOptions.Bounding >= MECHSIM_FACE_BOUNDING_BBOX)
6330 {
6331 if (Bounding_Data->intersect_list != NULL) POV_FREE(Bounding_Data->intersect_list);
6332 if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
6333 {
6334 if (Bounding_Data->bounding_hashtable != NULL)
6335 {
6336 for (i = 0; i < Bounding_Data->bounding_hashtable_size; i++)
6337 {
6338 if (Bounding_Data->bounding_hashtable[i].index != NULL)
6339 {
6340 POV_FREE(Bounding_Data->bounding_hashtable[i].index);
6341 }
6342 }
6343 POV_FREE(Bounding_Data->bounding_hashtable);
6344 }
6345 if (Bounding_Data->bitfield != NULL) POV_FREE(Bounding_Data->bitfield);
6346 }
6347 if (Bounding_Data->Faces_BBox_Array != NULL) POV_FREE(Bounding_Data->Faces_BBox_Array);
6348 if (Bounding_Data->Face_Groups_BBox_Array != NULL) POV_FREE(Bounding_Data->Face_Groups_BBox_Array);
6349 if (Bounding_Data->Mass_Groups_BBox_Array != NULL) POV_FREE(Bounding_Data->Mass_Groups_BBox_Array);
6350 if (Bounding_Data->Connect_Groups_BBox_Array != NULL) POV_FREE(Bounding_Data->Connect_Groups_BBox_Array);
6351 if (Bounding_Data->Connects_BBox_Array != NULL) POV_FREE(Bounding_Data->Connects_BBox_Array);
6352 POV_FREE(Bounding_Data);
6353 }
6354 }
6355
6356 /*****************************************************************************
6357 *
6358 * FUNCTION
6359 *
6360 * calc_mass_unit_size
6361 *
6362 * INPUT
6363 *
6364 * OUTPUT
6365 *
6366 * RETURNS Unit size.
6367 *
6368 * AUTHOUR
6369 *
6370 * Daniel Jungmann
6371 *
6372 * DESCRIPTION
6373 *
6374 * Berechnet die Gittergröße für die Hash-Tabelle als das Doppeltes des
6375 * durchschnittlichen Massenradius
6376 *
6377 * CHANGES
6378 *
6379 * --- April 2004 : Creation
6380 *
6381 ******************************************************************************/
6382
calc_mass_unit_size(void)6383 static SNGL calc_mass_unit_size(void)
6384 {
6385 int i;
6386 SNGL Unit;
6387
6388 Unit = 0.0;
6389
6390 for (i = 0; i < MechsimOptions.Data.mass_count; i++)
6391 Unit += MechsimOptions.Data.Masses[i].Radius;
6392
6393 Unit *= 2;
6394
6395 Unit /= MechsimOptions.Data.mass_count;
6396 return Unit;
6397 }
6398
6399 /*****************************************************************************
6400 *
6401 * FUNCTION
6402 *
6403 * calc_masses_bounding_hashtable
6404 *
6405 * INPUT
6406 * Pos_Array array of masses positions.
6407 * Bounding_Data all bounding data.
6408 *
6409 * OUTPUT
6410 *
6411 * RETURNS bounding hashtable for all masses.
6412 *
6413 * AUTHOUR
6414 *
6415 * Daniel Jungmann
6416 *
6417 * DESCRIPTION
6418 *
6419 * Calculates the bounding hashtable for all masses.
6420 *
6421 * CHANGES
6422 *
6423 * --- Mar 2004 : Creation
6424 *
6425 ******************************************************************************/
6426
calc_masses_bounding_hashtable(VECTOR * Pos_Array,MECHSIM_BOUNDING_DATA * Bounding_Data)6427 static void calc_masses_bounding_hashtable(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
6428 {
6429 BBOX BBox;
6430 long int jx_min, jy_min, jz_min;
6431 long int jx_max, jy_max, jz_max;
6432 long int jx, jy, jz;
6433 int i, hash;
6434
6435 for (i = 0; i < MechsimOptions.Data.mass_count; i++)
6436 {
6437 /* Bestimmung des minimalen Gitterpunktes der Kugel */
6438 jx_min = (long int)floor((Pos_Array[i][X]-MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[X]);
6439 jy_min = (long int)floor((Pos_Array[i][Y]-MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Y]);
6440 jz_min = (long int)floor((Pos_Array[i][Z]-MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Z]);
6441
6442 /* Bestimmung des maximalen Gitterpunktes der Kugel */
6443 jx_max = (long int)floor((Pos_Array[i][X]+MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[X]);
6444 jy_max = (long int)floor((Pos_Array[i][Y]+MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Y]);
6445 jz_max = (long int)floor((Pos_Array[i][Z]+MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Z]);
6446
6447 BBox.Lengths[X] = Bounding_Data->Unit[X];
6448 BBox.Lengths[Y] = Bounding_Data->Unit[Y];
6449 BBox.Lengths[Z] = Bounding_Data->Unit[Z];
6450 BBox.Lower_Left[X] = jx_min*Bounding_Data->Unit[X];
6451 for (jx = jx_min; jx <= jx_max; jx++)
6452 {
6453 BBox.Lower_Left[Y] = jy_min*Bounding_Data->Unit[Y];
6454 for (jy = jy_min; jy <= jy_max; jy++)
6455 {
6456 BBox.Lower_Left[Z] = jz_min*Bounding_Data->Unit[Z];
6457 for (jz = jz_min; jz <= jz_max; jz++)
6458 {
6459 /* We test if the sphere intersects this bounding box */
6460 if (intersect_sphere_box(BBox, Pos_Array[i], MechsimOptions.Data.Masses[i].Radius))
6461 {
6462 hash = bounding_hash_func(jx, jy, jz, Bounding_Data->bounding_hashtable_size);
6463 insert_index_bounding_hastable(Bounding_Data->bounding_hashtable, hash, i, Bounding_Data->time_stamp);
6464 }
6465 BBox.Lower_Left[Z] += Bounding_Data->Unit[Z];
6466 }
6467 BBox.Lower_Left[Y] += Bounding_Data->Unit[Y];
6468 }
6469 BBox.Lower_Left[X] += Bounding_Data->Unit[X];
6470 }
6471 }
6472 }
6473
6474 /*****************************************************************************
6475 *
6476 * FUNCTION
6477 *
6478 * calc_mass_groups_bounding_box
6479 *
6480 * INPUT
6481 * Pos_Array array of masses positions.
6482 * Bounding_Data all bounding data.
6483 *
6484 * OUTPUT
6485 *
6486 * RETURNS
6487 *
6488 * AUTHOUR
6489 *
6490 * Daniel Jungmann
6491 *
6492 * DESCRIPTION
6493 *
6494 * Berechnet die Bounding-Boxen für alle Gruppen. Dabei werden nur die Massen berücksichtigt.
6495 *
6496 * CHANGES
6497 *
6498 * --- April 2004 : Creation
6499 *
6500 ******************************************************************************/
6501
calc_mass_groups_bounding_box(VECTOR * Pos_Array,MECHSIM_BOUNDING_DATA * Bounding_Data)6502 static void calc_mass_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
6503 {
6504 VECTOR t_max, t_min, g_max, g_min;
6505 int Idx, i, j;
6506
6507 for (i = 0; i < MechsimOptions.Data.group_count; i++)
6508 {
6509 Make_Vector(g_max, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
6510 /* Die Gruppen enthält keine Massen, also kann die Bounding-Box die Größe Null haben und möglichst weit weg liegen. */
6511 if (MechsimOptions.Data.Groups[i].mass_count == 0)
6512 {
6513 Make_Vector(g_min, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
6514 Make_BBox_from_min_max(Bounding_Data->Mass_Groups_BBox_Array[i], g_min, g_max);
6515 }
6516 else
6517 {
6518 Make_Vector(g_min, BOUND_HUGE, BOUND_HUGE, BOUND_HUGE);
6519
6520 for (j = 0; j < MechsimOptions.Data.Groups[i].mass_count; j++)
6521 {
6522 Idx = MechsimOptions.Data.Groups[i].Masses[j]->Index;
6523 t_max[X] = Pos_Array[Idx][X] + MechsimOptions.Data.Masses[Idx].Radius;
6524 t_max[Y] = Pos_Array[Idx][Y] + MechsimOptions.Data.Masses[Idx].Radius;
6525 t_max[Z] = Pos_Array[Idx][Z] + MechsimOptions.Data.Masses[Idx].Radius;
6526 t_min[X] = Pos_Array[Idx][X] - MechsimOptions.Data.Masses[Idx].Radius;
6527 t_min[Y] = Pos_Array[Idx][Y] - MechsimOptions.Data.Masses[Idx].Radius;
6528 t_min[Z] = Pos_Array[Idx][Z] - MechsimOptions.Data.Masses[Idx].Radius;
6529
6530 g_max[X] = max(g_max[X], t_max[X]);
6531 g_max[Y] = max(g_max[Y], t_max[Y]);
6532 g_max[Z] = max(g_max[Z], t_max[Z]);
6533 g_min[X] = min(g_min[X], t_min[X]);
6534 g_min[Y] = min(g_min[Y], t_min[Y]);
6535 g_min[Z] = min(g_min[Z], t_min[Z]);
6536 }
6537 /* Bounding-Box der Gruppe. */
6538 Make_BBox_from_min_max(Bounding_Data->Mass_Groups_BBox_Array[i], g_min, g_max);
6539 }
6540 }
6541 }
6542
6543 /*****************************************************************************
6544 * Berechnet welche Massen in der Schnittmenge liegen. Nur die Massen, die
6545 * tatsächlich geschnitten werden, müssen später überprüft werde.
6546 ******************************************************************************/
calc_mass_group_intersection(VECTOR * Pos_Array,BBOX intersection_Box,int Grp,MECHSIM_BOUNDING_DATA * Bounding_Data)6547 static void calc_mass_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data)
6548 {
6549 int i, Idx, Index;
6550
6551 if (Bounding_Data->intersect_list_size < MechsimOptions.Data.Groups[Grp].mass_count)
6552 {
6553 Bounding_Data->intersect_list_size = MechsimOptions.Data.Groups[Grp].mass_count;
6554 Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list,
6555 Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding box intersect list");
6556 }
6557 Index = 0;
6558 for (i = 0; i < MechsimOptions.Data.Groups[Grp].mass_count; i++)
6559 {
6560 Idx = MechsimOptions.Data.Groups[Grp].Masses[i]->Index;
6561 if (intersect_sphere_box(intersection_Box, Pos_Array[Idx], MechsimOptions.Data.Masses[Idx].Radius))
6562 {
6563 Bounding_Data->intersect_list[Index] = Idx;
6564 Index++;
6565 }
6566 }
6567 Bounding_Data->intersect_list_index = Index;
6568 }
6569
6570 /*****************************************************************************
6571 *
6572 * FUNCTION
6573 *
6574 * calc_connect_groups_bounding_box
6575 *
6576 * INPUT
6577 * Pos_Array array of masses positions.
6578 * Bounding_Data all bounding data.
6579 *
6580 * OUTPUT
6581 *
6582 * RETURNS
6583 *
6584 * AUTHOUR
6585 *
6586 * Daniel Jungmann
6587 *
6588 * DESCRIPTION
6589 *
6590 * Berechnet die Bounding-Boxen für alle Gruppen. Dabei werden nur die Verbindungen berücksichtigt.
6591 *
6592 * CHANGES
6593 *
6594 * --- Sep 2004 : Creation
6595 *
6596 ******************************************************************************/
calc_connect_groups_bounding_box(VECTOR * Pos_Array,MECHSIM_BOUNDING_DATA * Bounding_Data)6597 static void calc_connect_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
6598 {
6599 VECTOR t_max, t_min, g_max, g_min;
6600 SNGL_VECT Basis[3], tmp_vect, tmp_vect1, tmp_vect2;
6601 SNGL tmp, Radius;
6602 int Idx1, Idx2, Index, i, j;
6603 int Dir1, Dir2, Dir3;
6604
6605 for (i = 0; i < MechsimOptions.Data.group_count; i++)
6606 {
6607 Make_Vector(g_max, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
6608 /* Die Gruppen enthält keine Verbindungen, also kann die Bounding-Box die Grö�e Null haben und möglichst weit weg liegen. */
6609 if (MechsimOptions.Data.Groups[i].connect_count == 0)
6610 {
6611 Make_Vector(g_min, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
6612 Make_BBox_from_min_max(Bounding_Data->Connect_Groups_BBox_Array[i], g_min, g_max);
6613 }
6614 else
6615 {
6616 Make_Vector(g_min, BOUND_HUGE, BOUND_HUGE, BOUND_HUGE);
6617
6618 for (j = 0; j < MechsimOptions.Data.Groups[i].connect_count; j++)
6619 {
6620 Index = MechsimOptions.Data.Groups[i].Connects[j]->Index;
6621 Idx1 = MechsimOptions.Data.Groups[i].Connects[j]->Idx1;
6622 Idx2 = MechsimOptions.Data.Groups[i].Connects[j]->Idx2;
6623 Radius = (MechsimOptions.Data.Masses[Idx1].Radius + MechsimOptions.Data.Masses[Idx2].Radius) / 2.0;
6624 t_max[X] = max(Pos_Array[Idx1][X], Pos_Array[Idx2][X]) + Radius;
6625 t_max[Y] = max(Pos_Array[Idx1][Y], Pos_Array[Idx2][Y]) + Radius;
6626 t_max[Z] = max(Pos_Array[Idx1][Z], Pos_Array[Idx2][Z]) + Radius;
6627 t_min[X] = min(Pos_Array[Idx1][X], Pos_Array[Idx2][X]) - Radius;
6628 t_min[Y] = min(Pos_Array[Idx1][Y], Pos_Array[Idx2][Y]) - Radius;
6629 t_min[Z] = min(Pos_Array[Idx1][Z], Pos_Array[Idx2][Z]) - Radius;
6630 Make_BBox_from_min_max(Bounding_Data->Connects_BBox_Array[Index], t_min, t_max);
6631
6632 g_max[X] = max(g_max[X], t_max[X]);
6633 g_max[Y] = max(g_max[Y], t_max[Y]);
6634 g_max[Z] = max(g_max[Z], t_max[Z]);
6635 g_min[X] = min(g_min[X], t_min[X]);
6636 g_min[Y] = min(g_min[Y], t_min[Y]);
6637 g_min[Z] = min(g_min[Z], t_min[Z]);
6638 }
6639 /* Bounding-Box der Gruppe. */
6640 Make_BBox_from_min_max(Bounding_Data->Connect_Groups_BBox_Array[i], g_min, g_max);
6641 }
6642 }
6643 }
6644
6645
6646 /*****************************************************************************
6647 * Berechnet welche Verbindungen in der Schnittmenge liegen. Nur die Verbindungen,
6648 * die tatsächlich geschnitten werden, müssen später überprüft werde.
6649 ******************************************************************************/
calc_connect_group_intersection(VECTOR * Pos_Array,BBOX intersection_Box,int Grp,MECHSIM_BOUNDING_DATA * Bounding_Data)6650 static void calc_connect_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data)
6651 {
6652 int i, Idx, Index;
6653
6654 if (Bounding_Data->intersect_list_size < MechsimOptions.Data.Groups[Grp].connect_count)
6655 {
6656 Bounding_Data->intersect_list_size = MechsimOptions.Data.Groups[Grp].connect_count;
6657 Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list,
6658 Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding box intersect list");
6659 }
6660 Index = 0;
6661 for (i = 0; i < MechsimOptions.Data.Groups[Grp].connect_count; i++)
6662 {
6663 Idx = MechsimOptions.Data.Groups[Grp].Connects[i]->Index;
6664 if (intersect_box_box(intersection_Box, Bounding_Data->Connects_BBox_Array[Idx]))
6665 {
6666 Bounding_Data->intersect_list[Index] = Idx;
6667 Index++;
6668 }
6669 }
6670 Bounding_Data->intersect_list_index = Index;
6671 }
6672
6673 /*****************************************************************************
6674 * Inline functions
6675 ******************************************************************************/
6676
6677 /*****************************************************************************
6678 *
6679 * FUNCTION
6680 *
6681 * bounding_hash_func
6682 *
6683 * INPUT
6684 *
6685 * OUTPUT
6686 *
6687 * RETURNS
6688 *
6689 * int - hash value of the position
6690 *
6691 * AUTHOR
6692 *
6693 * Daniel Jungmann
6694 *
6695 * DESCRIPTION
6696 *
6697 * -
6698 *
6699 * CHANGES
6700 *
6701 * --- Mar 2004 : Creation
6702 *
6703 ******************************************************************************/
6704
bounding_hash_func(long int x,long int y,long int z,long int n)6705 inline unsigned int bounding_hash_func(long int x, long int y, long int z, long int n)
6706 {
6707 long int r;
6708 unsigned int ret;
6709 x *= 73856093L;
6710 y *= 19349663L;
6711 z *= 83492791L;
6712 r = x ^ y ^ z;
6713 r %= n;
6714 ret = abs(r);
6715 return ret;
6716 }
6717
6718 // ========================================================================
6719
6720 END_POV_NAMESPACE
6721
6722 #endif
6723