/****************************************************************************
 *                  mechsim.cpp
 *
 *  Author: Christoph Hormann <chris_hormann@gmx.de>
 *
 *  Mechanics simulation
 *
 *  Copyright 2002-2005 Christoph Hormann
 *
 * from Persistence of Vision(tm) Ray Tracer version 3.6.
 * Copyright 1991-2003 Persistence of Vision Team
 * Copyright 2003-2004 Persistence of Vision Raytracer Pty. Ltd.
 *---------------------------------------------------------------------------
 * NOTICE: This source code file is provided so that users may experiment
 * with enhancements to POV-Ray and to port the software to platforms other
 * than those supported by the POV-Ray developers. There are strict rules
 * regarding how you are permitted to use this file. These rules are contained
 * in the distribution and derivative versions licenses which should have been
 * provided with this file.
 *
 * These licences may be found online, linked from the end-user license
 * agreement that is located at http://www.povray.org/povlegal.html
 *---------------------------------------------------------------------------
 * This program is based on the popular DKB raytracer version 2.12.
 * DKBTrace was originally written by David K. Buck.
 * DKBTrace Ver 2.0-2.12 were written by David K. Buck & Aaron A. Collins.
 *---------------------------------------------------------------------------
 *
 *===========================================================================
 * This file is part of MegaPOV, a modified and unofficial version of POV-Ray
 * For more information on MegaPOV visit our website:
 * http://megapov.inetart.net/
 *===========================================================================
 *
 * $RCSfile: mechsim.cpp,v $
 * $Revision: 1.27 $
 * $Author: chris $
 *
 *****************************************************************************/

/*

  The MechSim patch
  =================

	History:

	  SimPOV 0.1.0:   October 2002
		-----------------------------------

		  - the basics: masses, connections, faces, environments


		SimPOV 0.2.0:   December 2002
    -----------------------------------

		  - bugfixes


		MegaPOV 1.0:    December 31, 2002
    -----------------------------------

		  - adding attachments, fields, interactions
			- other improvements


		MegaPOV 1.1:    December 31, 2002
    -----------------------------------

		  - Connection-connection collisions
			- collision bounding/hashing system by Daniel Jungmann


		MegaPOV 1.2:
    -----------------------------------

			- improved collision bounding/hashing system by Daniel Jungmann
      - viscoelastic connections
      - global fixation in 1 or 2 dimensions
      - attachments can be limited to 1 or two dimensions
        (free movement in the others)
      - adaptive time stepping for 4th order Runge-Kutta method
      - new simulation data file format
      - custom forces that can be applied to individual masses
      - more detailed simulation statistics
      - access to simulation forces from SDL
      - various bug fixes and improvements


*/

#include "frame.h"
#include "povray.h"
#include "vector.h"
#include "function.h"
#include "fnpovfpu.h"
#include "pov_util.h"
#include "mesh.h"
#include "ray.h"
#include "objects.h"
#include "povmsend.h"
#include "mechsim.h"

#include <algorithm>

#ifdef MECHANICS_SIMULATION_PATCH            /* Christoph Hormann August 2002 */

BEGIN_POV_NAMESPACE

/*****************************************************************************
* Local preprocessor defines
******************************************************************************/

//#define MECHSIM_DEBUG 1

#define MAX_STR 1024

#define MECHSIM_GRADIENT_ACCURACY 0.001

#define MECHSIM_EPSILON 0.0001

#define MECHSIM_BOUNDING_HASH_THRESHOLD 100
#define MECHSIM_BOUNDING_BOX_THRESHOLD 10

// bounding

#define INT		unsigned int

#define testbit(ss, ibit) (((ss)[BIT2INT(ibit)] & BIT2MASK(ibit)) != 0)
#define setbit(ss, ibit) ((ss)[BIT2INT(ibit)] |= BIT2MASK(ibit))

#define BITSPERINT	(8*sizeof(INT))
#define NINTS(nbits)	(((nbits) + BITSPERINT - 1) / BITSPERINT)

#define BIT2INT(ibit)	((ibit) / BITSPERINT)
#define BIT2SHIFT(ibit)	((ibit) % BITSPERINT)
#define BIT2MASK(ibit)	(1 << BIT2SHIFT(ibit))

// adaptive time stepping

const DBL Mechsim_Safety_Fact = 0.9;
const DBL Mechsim_Exp_Inc = 0.2;
const DBL Mechsim_Exp_Dec = 0.25;
const DBL Mechsim_Error_Threshold = pow(5.0/Mechsim_Safety_Fact, -1.0/Mechsim_Exp_Inc);


/*****************************************************************************
* Local functions
******************************************************************************/

void Function_Gradient(FUNCTION funct, VECTOR Pos, VECTOR grad);
int func_triangle(VECTOR Point, VECTOR CPoint, VECTOR Grad, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Ratio);
void Triangle_Distribute(VECTOR Point, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Weight1, DBL *Weight2, DBL *Weight3);
void Calc_Friction(VECTOR FFrict, VECTOR Velocity, DBL Friction, VECTOR Normal, DBL Force);

void Mechsim_Impact_Collide_Environment(int Index, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env);
void Mechsim_Gradient_Collide_Environment(int Index, VECTOR Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env);
void Mechsim_Force_Collide_Environment(int Index, VECTOR Accel,VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env);

void Mechsim_Collide_Dynamic(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
void Mechsim_Collide_Mass_Mass(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2);
void Mechsim_Collide_Mass_Face(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2);
void Mechsim_Collide_Connection_Connection(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2);

void Mechsim_Gradient_Connect(VECTOR *Gradient, VECTOR *Pos_Array);
void Mechsim_Force_Connect(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array);

void Mechsim_Gradient_Viscoelastics(VECTOR *Gradient, VECTOR *Pos_Array);
void Mechsim_Force_Viscoelastics(VECTOR *Accel, VECTOR *State_Array);
void Mechsim_Integrate_Viscoelastics(VECTOR *State_Array);

void Mechsim_Gradient_Interactions(VECTOR *Accel, VECTOR *Pos_Array);
void Mechsim_Force_Interactions(VECTOR *Accel, VECTOR *Pos_Array);

void Mechsim_Force_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array);
void Mechsim_Gradient_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array);

void Mechsim_Force_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array);
void Mechsim_Gradient_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array);

void Mechsim_Gradients(VECTOR *Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data);
void Mechsim_Forces(VECTOR *Forces, VECTOR *State_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data);
void Mechsim_Check_Col(VECTOR Pos, VECTOR Old_Pos);
void Mechsim_Check_Instability(VECTOR Pos, VECTOR Vel, VECTOR Accel);

void Mechsim_Integrate_Euler(VECTOR *State_Array, VECTOR *Forces);
void Mechsim_Integrate_Heun(VECTOR *State_Array, VECTOR *Forces);
void Mechsim_Integrate_Runge_Kutta4(VECTOR *State_Array, VECTOR *Forces);
void Mechsim_Integrate_Gradient(VECTOR *State_Array, VECTOR *Forces);

void Mechsim_Create_Groups(int Count);
void Mechsim_Prepare_Data(void);
bool Mechsim_save_file (void);

char *Read_Vector(IStream *fd, char *str, char *buffer, VECTOR Vect, bool *got_eof);
char *Read_Float(IStream *fd, char *str, char *buffer, double *Value, bool *got_eof);
char *Read_Int(IStream *fd, char *str, char *buffer, int *Value, bool *got_eof);

static void calc_faces_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
static bool intersect_sphere_box(BBOX BBox, VECTOR Center, DBL Radius);
static void calc_faces_bounding_hashtable(MECHSIM_BOUNDING_DATA* Bounding_Data);
static void insert_index_bounding_hastable(BOUNDING_HASHTABLE_ITEM* bounding_hashtable, unsigned int hash, int index, int time_stamp);
static void intersect_sphere_bounding_hashtable(VECTOR Center, DBL Radius, MECHSIM_BOUNDING_DATA* Bounding_Data);
static void calc_face_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
static unsigned int bounding_hash_func(long int x, long int y, long int z, long int n);
static bool intersect_box_box(BBOX BBox_1, BBOX BBox_2);
static void calc_face_group_intersection(BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data);
static MECHSIM_BOUNDING_DATA* init_bounding(void);
static void free_bounding(MECHSIM_BOUNDING_DATA* Bounding_Data);
static SNGL calc_mass_unit_size(void);
static void calc_masses_bounding_hashtable(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
static void calc_mass_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
static void calc_mass_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data);
static void calc_connect_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data);
static void calc_connect_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data);

/* ------------------------------------------------------ */
/* global variables */
/* ------------------------------------------------------ */

MECHSIM_OPTIONS MechsimOptions;

/* ------------------------------------------------------ */
/* external variables */
/* ------------------------------------------------------ */

/* ------------------------------------------------------ */
/* static functions */
/* ------------------------------------------------------ */

/* ------------------------------------------------------ */
/* static variables */
/* ------------------------------------------------------ */

/*****************************************************************************
*
* FUNCTION
*
*   Initialize_MechSim
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Set default values of simulation data structure
*
* CHANGES
*
*   --- Aug 2002 : Creation
*
******************************************************************************/
void Initialize_MechSim(void)
{
  /* Initialize 'MechsimOptions' fields */

  MechsimOptions.Method = 1;
  MechsimOptions.Step_Count = 10;
  MechsimOptions.Time_Step = 0.1;
	MechsimOptions.Time_Step_Min = 1.0e-7;
	MechsimOptions.Accuracy = 1.0e-5;
  MechsimOptions.Start_Time = 0.0;
  MechsimOptions.End_Time = 0.0;
  MechsimOptions.Time = 0.0;
	MechsimOptions.TS_Min = 0.1; 
	MechsimOptions.TS_Max = 0.1; 
	MechsimOptions.Timing_Given = false;
	MechsimOptions.Timing_Loaded = false;
  Make_Vector(MechsimOptions.gravity, 0.0, 0.0, 0.0);
  MechsimOptions.Damping = 0.0;
  MechsimOptions.Environment = NULL;
  MechsimOptions.Env_max_count = 0;
  MechsimOptions.Env_count = 0;
  MechsimOptions.Collision_Stiffness = 0.0;
  MechsimOptions.Collision_Damping = 0.0;
  MechsimOptions.Collision_Friction = 0.0;
  MechsimOptions.Collision_Friction_Excess = 1.0;
  MechsimOptions.Calc_Collision = MECHSIM_COLLISION_NONE;
  MechsimOptions.Calc_Collision_Faces = MECHSIM_COLLISION_NONE;
  MechsimOptions.Calc_Collision_Connections = MECHSIM_COLLISION_NONE;
  MechsimOptions.Load_File_Name = NULL;
  MechsimOptions.Save_File_Name = NULL;
  MechsimOptions.Save_File = false;
  MechsimOptions.Save_File_Type = 4;
  MechsimOptions.Data.mass_count = 0;
  MechsimOptions.Data.connect_count = 0;
  MechsimOptions.Data.faces_count = 0;
  MechsimOptions.Data.ve_count = 0;
  MechsimOptions.Data.group_count = 0;

  MechsimOptions.Data.max_masses = 0;
  MechsimOptions.Data.max_connects = 0;
  MechsimOptions.Data.max_faces = 0;
  MechsimOptions.Data.max_groups = 0;
  MechsimOptions.Data.max_viscoelastics = 0;

  MechsimOptions.Enabled = false;

  MechsimOptions.Interactions.Function = NULL;
  MechsimOptions.Interactions.count = 0;
  MechsimOptions.Interactions.max_count = 0;

  MechsimOptions.Attachments.Function = NULL;
  MechsimOptions.Attachments.Fixed = NULL;
  MechsimOptions.Attachments.count = 0;
  MechsimOptions.Attachments.max_count = 0;

  MechsimOptions.Fields.Function = NULL;
  MechsimOptions.Fields.count = 0;
  MechsimOptions.Fields.max_count = 0;

  MechsimOptions.Forces.Function = NULL;
  MechsimOptions.Forces.count = 0;
  MechsimOptions.Forces.max_count = 0;

  MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_NO;
}


/*****************************************************************************
*
* FUNCTION
*
*   Deinitialize_MechSim
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Free all simulation data
*
* CHANGES
*
*   --- Aug 2002 : Creation

******************************************************************************/
void Deinitialize_MechSim(void)
{
  int i;

  if (MechsimOptions.Enabled)
  {
    if (MechsimOptions.Data.ve_count>0)
		{
      for(i=0; i<MechsimOptions.Data.ve_count; i++)
      {
				POV_FREE(MechsimOptions.Data.Viscoelastics[i].Element);
			}
		}

		MechsimOptions.Data.mass_count = 0;
		MechsimOptions.Data.connect_count = 0;
		MechsimOptions.Data.faces_count = 0;
		MechsimOptions.Data.ve_count = 0;
		MechsimOptions.Data.group_count = 0;

    if (MechsimOptions.Data.max_masses>0) POV_FREE(MechsimOptions.Data.Masses);
    if (MechsimOptions.Data.max_connects>0) POV_FREE(MechsimOptions.Data.Connects);
    if (MechsimOptions.Data.max_faces>0) POV_FREE(MechsimOptions.Data.Faces);
    if (MechsimOptions.Data.max_viscoelastics>0) POV_FREE(MechsimOptions.Data.Viscoelastics);

    if (MechsimOptions.Data.max_groups>0)
    {
      for(i=0; i<MechsimOptions.Data.max_groups; i++)
      {
        POV_FREE(MechsimOptions.Data.Groups[i].Masses);
        POV_FREE(MechsimOptions.Data.Groups[i].Connects);
        POV_FREE(MechsimOptions.Data.Groups[i].Faces);
      }

      POV_FREE(MechsimOptions.Data.Groups);
    }

    MechsimOptions.Data.max_masses = 0;
    MechsimOptions.Data.max_connects = 0;
    MechsimOptions.Data.max_faces = 0;
    MechsimOptions.Data.max_viscoelastics = 0;
    MechsimOptions.Data.max_groups = 0;

    if (MechsimOptions.Interactions.max_count>0)
		{
			for (i=0; i<MechsimOptions.Interactions.count; i++)
				Destroy_Function(MechsimOptions.Interactions.Function[i]);
			POV_FREE(MechsimOptions.Interactions.Function);
		}
    if (MechsimOptions.Attachments.max_count>0) 
		{
			for (i=0; i<MechsimOptions.Attachments.count; i++)
				Destroy_Function(MechsimOptions.Attachments.Function[i]);
			POV_FREE(MechsimOptions.Attachments.Function);
			POV_FREE(MechsimOptions.Attachments.Fixed);
		}
    if (MechsimOptions.Fields.max_count>0) 
		{
			for (i=0; i<MechsimOptions.Fields.count; i++)
				Destroy_Function(MechsimOptions.Fields.Function[i]);
			POV_FREE(MechsimOptions.Fields.Function);
		}
    if (MechsimOptions.Forces.max_count>0) 
		{
			for (i=0; i<MechsimOptions.Forces.count; i++)
				Destroy_Function(MechsimOptions.Forces.Function[i]);
			POV_FREE(MechsimOptions.Forces.Function);
		}

    MechsimOptions.Attachments.max_count = 0;
    MechsimOptions.Attachments.count = 0;
    MechsimOptions.Interactions.max_count = 0;
    MechsimOptions.Interactions.count = 0;
    MechsimOptions.Fields.max_count = 0;
    MechsimOptions.Fields.count = 0;
    MechsimOptions.Forces.max_count = 0;
    MechsimOptions.Forces.count = 0;

    MechsimOptions.Enabled = false;
  }

  /* Environments */
  if (MechsimOptions.Env_max_count>0)
  {
		for (i=0; i<MechsimOptions.Env_count; i++)
		{
			if (MechsimOptions.Environment[i].Function != NULL) Destroy_Function(MechsimOptions.Environment[i].Function);
			if (MechsimOptions.Environment[i].Object != NULL) Destroy_Object(MechsimOptions.Environment[i].Object);
			MechsimOptions.Environment->Function = NULL;
			MechsimOptions.Environment->Object = NULL;
		}
    POV_FREE(MechsimOptions.Environment);
    MechsimOptions.Environment = NULL;
		MechsimOptions.Env_count = 0;
		MechsimOptions.Env_max_count = 0;
  }

}

/*****************************************************************************
*
* FUNCTION
*
*   Function_Gradient
*
* INPUT
*
*   funct - function to calculate gradient from
*   Pos - evaluation point
*
* OUTPUT
*
*   the gradient is returned via 'grad'
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the gradient of a function at a specified point.
*
*   Works just like the corresponding macro in 'math.inc'
*
* CHANGES
*
*   --- Aug 2002 : Creation
*
******************************************************************************/

void Function_Gradient(FUNCTION funct, VECTOR Pos, VECTOR grad)
{

  POVFPU_SetLocal(X, Pos[X] + MECHSIM_GRADIENT_ACCURACY);
  POVFPU_SetLocal(Y, Pos[Y]);
  POVFPU_SetLocal(Z, Pos[Z]);
  grad[X] = POVFPU_Run(funct);
  POVFPU_SetLocal(X, Pos[X] - MECHSIM_GRADIENT_ACCURACY);
  POVFPU_SetLocal(Y, Pos[Y]);
  POVFPU_SetLocal(Z, Pos[Z]);
  grad[X] -= POVFPU_Run(funct);
  grad[X] /= 2.0*MECHSIM_GRADIENT_ACCURACY;

  POVFPU_SetLocal(X, Pos[X]);
  POVFPU_SetLocal(Y, Pos[Y] + MECHSIM_GRADIENT_ACCURACY);
  POVFPU_SetLocal(Z, Pos[Z]);
  grad[Y] = POVFPU_Run(funct);
  POVFPU_SetLocal(X, Pos[X]);
  POVFPU_SetLocal(Y, Pos[Y] - MECHSIM_GRADIENT_ACCURACY);
  POVFPU_SetLocal(Z, Pos[Z]);
  grad[Y] -= POVFPU_Run(funct);
  grad[Y] /= 2.0*MECHSIM_GRADIENT_ACCURACY;

  POVFPU_SetLocal(X, Pos[X]);
  POVFPU_SetLocal(Y, Pos[Y]);
  POVFPU_SetLocal(Z, Pos[Z] + MECHSIM_GRADIENT_ACCURACY);
  grad[Z] = POVFPU_Run(funct);
  POVFPU_SetLocal(X, Pos[X]);
  POVFPU_SetLocal(Y, Pos[Y]);
  POVFPU_SetLocal(Z, Pos[Z] - MECHSIM_GRADIENT_ACCURACY);
  grad[Z] -= POVFPU_Run(funct);
  grad[Z] /= 2.0*MECHSIM_GRADIENT_ACCURACY;

}


/*****************************************************************************
*
* FUNCTION
*
*   func_triangle
*
* INPUT
*
*   Point - point to evaluate the triangle function at
*   P1, P2, P3 - vertices of the triangle
*
* OUTPUT
*
*   the distance of Point to the specified triangle
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the distance of a point to a triangle.
*
*   Derieved from the internal function 'f_triangle' developed by ABX
*
* CHANGES
*
*   --- Sep 2002 : Creation
*
******************************************************************************/

int func_triangle(VECTOR Point, VECTOR CPoint, VECTOR Grad, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Ratio)
{
        VECTOR v_temp,N,R1,R2,R3,E;
        DBL Nd,Ed,El, Dist;
        //VECTOR Distance;
        int Result;

        VSub(R1,P2,P1);
        VSub(R2,P3,P2);
        VSub(R3,P1,P3);
        VNormalizeCrossNotZeroLength(N,R2,R3);
        VNormalizeCrossNotZeroLength(E,N,R1);
        VDot(Ed,E,P1);
        VDot(El,E,Point);
        if ( Ed < El )
        {
          VNormalizeCrossNotZeroLength(E,N,R2);
          VDot(Ed,E,P2);
          VDot(El,E,Point);
          if ( Ed < El )
          {
            VNormalizeCrossNotZeroLength(E,N,R3);
            VDot(Ed,E,P3);
            VDot(El,E,Point);
            if ( Ed < El )
            {
              VDot(Nd,N,P1);
              VDot(Dist,N,Point);
              //Distance = fabs( Distance-Nd );
              VScale(Grad, N, Dist-Nd);
              VAddScaled(CPoint, Point, Nd-Dist, N);
              Result = 0;
            }
            else
            {
              //DistanceToEdge( Distance , R3 , P3 , Point );
              VToEdge( Grad , R3 , P3 , Point );
              VSub(v_temp, Point, P3);
              VNormalizeEq(R3);
              VDot(*Ratio,v_temp,R3);
              Result = 3;
            }
          }
          else
          {
            //DistanceToEdge( Distance , R2 , P2 , Point );
            VToEdge( Grad , R2 , P2 , Point );
            VSub(v_temp, Point, P2);
            VNormalizeEq(R2);
            VDot(*Ratio,v_temp,R2);
            Result = 2;
          }
        }
        else
        {
          //DistanceToEdge( Distance , R1 , P1 , Point );
          VToEdge( Grad , R1 , P1 , Point );
          VSub(v_temp, Point, P1);
          VNormalizeEq(R1);
          VDot(*Ratio,v_temp,R1);
          Result = 1;
        }

        return Result;
}

/*****************************************************************************
*
* FUNCTION
*
*   Triangle_Distribute
*
* INPUT
*
*   Point - impact point
*   P1, P2, P3 - vertices of the triangle
*
* OUTPUT
*
*   Weight1, Weight2, Weight3 - factors for weighting
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Weight the force on the triangle vertices according to static equilibrium
*
* CHANGES
*
*   --- Sep 2002 : Creation
*       Dec 2002 : Used a simpler method and probably fixed some problems
*
******************************************************************************/
void Triangle_Distribute(VECTOR Point, VECTOR P1, VECTOR P2, VECTOR P3, DBL *Weight1, DBL *Weight2, DBL *Weight3)
{
  DBL DSqSum1, DSqSum2, DSqSum3;
  VECTOR Dir1, Dir2, Dir3;
  DBL Val1, Val2, Val3;
  DBL Scale;

  /*
   * The method used here calculates the distance of the collision point
   * to the opposite side of the triangle.  With normalized scaling (i.e.
   * distances between 0 and 1 the sum of all distances has to be 1.0.
   * We just have to scale the distances with their sum and get correct
   * weights for the force.
   */

  VSub(Dir1, P2, P1);
  DSqSum1 = VSumSqr(Dir1);

  VSub(Dir2, P3, P2);
  DSqSum2 = VSumSqr(Dir2);

  VSub(Dir3, P3, P1);
  DSqSum3 = VSumSqr(Dir3);

  /* calculate the distance of the collision point to each side */
  Val1=(Point[X] - P1[X])*Dir1[Y] - (Point[Y] - P1[Y])*Dir1[X];
  Val2=(Point[Y] - P1[Y])*Dir1[Z] - (Point[Z] - P1[Z])*Dir1[Y];
  Val3=(Point[Z] - P1[Z])*Dir1[X] - (Point[X] - P1[X])*Dir1[Z];

  (*Weight1) = sqrt((Val1*Val1 + Val2*Val2 + Val3*Val3)/DSqSum1);

  Val1=(Point[X] - P2[X])*Dir2[Y] - (Point[Y] - P2[Y])*Dir2[X];
  Val2=(Point[Y] - P2[Y])*Dir2[Z] - (Point[Z] - P2[Z])*Dir2[Y];
  Val3=(Point[Z] - P2[Z])*Dir2[X] - (Point[X] - P2[X])*Dir2[Z];

  (*Weight2) = sqrt((Val1*Val1 + Val2*Val2 + Val3*Val3)/DSqSum2);

  Val1=(Point[X] - P1[X])*Dir3[Y] - (Point[Y] - P1[Y])*Dir3[X];
  Val2=(Point[Y] - P1[Y])*Dir3[Z] - (Point[Z] - P1[Z])*Dir3[Y];
  Val3=(Point[Z] - P1[Z])*Dir3[X] - (Point[X] - P1[X])*Dir3[Z];

  (*Weight3) = sqrt((Val1*Val1 + Val2*Val2 + Val3*Val3)/DSqSum3);

  /* calculate the sum of weights */
  Scale = 1.0/((*Weight1) + (*Weight2) + (*Weight3));

  /* scale the weights */
  (*Weight1) *= Scale;
  (*Weight2) *= Scale;
  (*Weight3) *= Scale;

  //Status_Info("\n%g Weight=<%g, %g, %g>\n", (*Weight1) + (*Weight2) + (*Weight3), (*Weight1), (*Weight2), (*Weight3) );

}


/*****************************************************************************
*
* FUNCTION
*
*   Calc_Friction
*
* INPUT
*
*   Velocity - relative velocity to calculate the friction for
*   Friction - friction parameters
*   Normal - Normal vector of the friction plane
*   Force - amount of normal (elastic) force
*
* OUTPUT
*
*   FFrict - Friction force (VECTOR)
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculate the friction force for various collision events
*
* CHANGES
*
*   --- Jan 2003 : Creation
*
******************************************************************************/
void Calc_Friction(VECTOR FFrict, VECTOR Velocity, DBL Friction, VECTOR Normal, DBL Force)
{
  //if (Friction > 0.0)
  //{

    DBL Len;
    VECTOR pot_force, FDir, Dir2;

    // friction already slightly above the surface
    VScale(pot_force, Normal, max(0.0, Force));

    VLength(Len, Velocity);
    if (Len>MECHSIM_EPSILON)
    {
      VCross(Dir2, Normal, Velocity);
      VCross(FDir, Dir2, Normal);
      VLength(Len, FDir);
      if (Len>MECHSIM_EPSILON) VInverseScaleEq(FDir, Len);

      VLength(Len, pot_force);
      VScale(FFrict, FDir, Friction*Len);
    }
    else Make_Vector(FFrict, 0.0, 0.0, 0.0);

  //}
  //else Make_Vector(FFrict, 0.0, 0.0, 0.0);
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Impact_Collide_Environment
*
* INPUT
*
*   Index - index of the mass
*   Pos_Array, Vel_Array - state of the masses
*   Time - current time index for the environment function
*   Env - Pointer to Environment
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates collision with the environment based on impact laws.
*
*   * If an environment function is given, it is evaluated and if
*     collision is found the gradient is calculated and the velocity
*     is mirrored at the surface and the position is updated.
*   * If only an object is given a ray is shot from the current position
*     in movement direction to determine the collision.
*
*   Friction and damping are applied as diminishing factors (see also
*   Christophe Bouffartique's cloth simulation)
*
*   ========== WARNING ====================================================
*
*   This does not necessarily work well with all integration methods,
*   The state of the system (i.e. positions and velocities) is modified
*   in this function instead of applying accelerations like we should do.
*
*   The object based method is also problematic with higher order
*   integration methods.  Apart from that it won't successfully find the
*   intersection in all situations like the function based method.
*
*   =======================================================================
*
* CHANGES
*
*   --- Sep 2002 : Creation
*       Oct 2002 : added object intersection based method
*
******************************************************************************/

void Mechsim_Impact_Collide_Environment(int Index, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env)
{
  DBL Norm_Len, Fn_Val;
  VECTOR Grad;

  if (Env->Function != NULL)  // ======== if function is given use it to calculate gradient ===========
  {
    FunctionCode *f = POVFPU_GetFunction(*Env->Function);

    // ---- collision test ----
    POVFPU_SetLocal(X, Pos_Array[Index][X]);
    POVFPU_SetLocal(Y, Pos_Array[Index][Y]);
    POVFPU_SetLocal(Z, Pos_Array[Index][Z]);
    if (f->parameter_cnt > 3) POVFPU_SetLocal(T, Time); // additional time parameter

    Fn_Val = POVFPU_Run(*Env->Function);

    if (Fn_Val < MechsimOptions.Data.Masses[Index].Radius)
    {
      Function_Gradient( *Env->Function, Pos_Array[Index], Grad);

      VNormalizeEq(Grad);

      /* now we do the bad thing and modify the velocity */
      VDot(Norm_Len, Vel_Array[Index], Grad);
      Norm_Len = -Norm_Len;
      VAddScaledEq(Vel_Array[Index], Norm_Len, Grad);  // make movement perpendicular to surface zero
      VScaleEq(Vel_Array[Index], 1.0-Env->Friction);   // apply friction
      VAddScaledEq(Vel_Array[Index], Norm_Len*Env->Damping, Grad);

      Increase_Counter(stats[MechSim_Environment_Collisions]);
    }
  }
  else if (Env->Object != NULL) // ======== if only object is given use intersection test ===========
  {
    INTERSECTION Intersect;
    RAY Ray;
    VECTOR Dir, Norm;
    DBL speed, dist;

    Initialize_Ray_Containers(&Ray);

    VNormalize(Dir, Vel_Array[Index]);
    Assign_Vector(Ray.Direction, Dir);
    Assign_Vector(Ray.Initial, Pos_Array[Index]);

    if(Intersection(&Intersect, Env->Object, &Ray))
    {
      VLength(speed, Vel_Array[Index]);
      VDist(dist, Intersect.IPoint, Pos_Array[Index]);
      // this does only make sense for single step (euler) integration method
      // but it could lead to agreeable result with other methods too
      if (dist < (MechsimOptions.Time_Step*speed + MechsimOptions.Data.Masses[Index].Radius))
      {
        Normal(Norm, Intersect.Object, &Intersect);
        VNormalizeEq(Norm);

        /* now we do the bad thing and modify the velocity */
        VDot(Norm_Len, Vel_Array[Index], Norm);
        Norm_Len = -Norm_Len;
        VAddScaledEq(Vel_Array[Index], Norm_Len, Norm);  // make movement perpendicular to surface zero
        VScaleEq(Vel_Array[Index], 1.0-Env->Friction);   // apply friction
        VAddScaledEq(Vel_Array[Index], Norm_Len*Env->Damping, Norm);

        Increase_Counter(stats[MechSim_Environment_Collisions]);
      }
    }

  }

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradient_Collide_Environment
*
* INPUT
*
*   Index - index of the mass
*   Pos_Array - state of the masses
*   Vel_Array - *** Different usage here ***
*               stores the last position of the mass
*   Time - current time index for the environment function
*   Env - Pointer to Environment
*
* OUTPUT
*
*   calculated gradients of the potential fields are added to 'Gradient'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Variation of Mechsim_Force_Collide_Environment() for use with gradient
*   descent method.  No damping forces.
*
* CHANGES
*
*   --- Sep 2002 : Creation
*       Oct 2002 : added object intersection based method
*
******************************************************************************/

void Mechsim_Gradient_Collide_Environment(int Index, VECTOR Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env)
{
  DBL Potential, Fn_Val;
  VECTOR pot_force, Grad;

  if (Env->Function != NULL)
  {
    FunctionCode *f = POVFPU_GetFunction(*Env->Function);

    // ---- collision test ----
    POVFPU_SetLocal(X, Pos_Array[Index][X]);
    POVFPU_SetLocal(Y, Pos_Array[Index][Y]);
    POVFPU_SetLocal(Z, Pos_Array[Index][Z]);
    if (f->parameter_cnt > 3) POVFPU_SetLocal(T, Time); // additional time parameter

    Fn_Val = POVFPU_Run(*Env->Function);

    if (Fn_Val < MechsimOptions.Data.Masses[Index].Radius)
    {
      Function_Gradient( *Env->Function, Pos_Array[Index], Grad);
      VNormalizeEq(Grad);

      // --- potential force ---
      Potential = max(0.0, MechsimOptions.Data.Masses[Index].Radius - Fn_Val);
      VScale(pot_force, Grad, Potential*Env->Stiffness);

      VAddEq(Gradient, pot_force);

      Increase_Counter(stats[MechSim_Environment_Collisions]);
    }
  }
  else if (Env->Object != NULL) // ======== if only object is given use intersection test ===========
  {
    INTERSECTION Intersect;
    RAY Ray;
    VECTOR Dir, Norm;
    DBL Len;

    Initialize_Ray_Containers(&Ray);

    // Vel_Array[Index] stores the position from the last step so:
    // direction = Pos_Array[Index] - Vel_Array[Index]
    // see also: Mechsim_Simulate()
    VSub(Dir, Pos_Array[Index], Vel_Array[Index]);

    if ((Dir[X] != 0.0) || (Dir[Y] != 0.0) || (Dir[Z] != 0.0))
    {
      Assign_Vector(Ray.Direction, Dir);
      Assign_Vector(Ray.Initial, Pos_Array[Index]);

      if(Intersection(&Intersect, Env->Object, &Ray))
      {
        Normal(Norm, Intersect.Object, &Intersect);      // get the normal
        VNormalizeEq(Norm);
        VSub(Dir, Intersect.IPoint, Pos_Array[Index]);   // approximate the distance (assuming flat surface)
        VDot(Len, Norm, Dir);

        // see if we need to invert the normal
        if (Len<0)
        {
          VScaleEq(Norm, -1);
          Len = -Len;
        }

        if (Len < MechsimOptions.Data.Masses[Index].Radius)
        {
          // --- potential force ---
          Potential = min(0.0, Len - MechsimOptions.Data.Masses[Index].Radius);
          VScale(pot_force, Norm, Potential*Env->Stiffness / MechsimOptions.Data.Masses[Index].Mass);

          VAddEq(Gradient, pot_force);

          Increase_Counter(stats[MechSim_Environment_Collisions]);

        }
      }
    }

  }

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Force_Collide_Environment
*
* INPUT
*
*   Index - index of the mass
*   Pos_Array, Vel_Array - state of the masses
*   Time - current time index for the environment function
*   Env - Pointer to Environment
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from a collision with the environment.
*
*   * If an environment function is given, it is evaluated and if
*     collision is found the gradient is calculated and an elastic force
*     is applied in direction of the gradient of the environment field.
*   * If only an object is given a ray is shot from the current position
*     in movement direction to determine the collision.
*
*   Damping and Friction are applied, Friction already slightly above the
*   surface to avoid gliding.
*
*   ========== WARNING ====================================================
*
*   The object based method is problematic because it relies on the velocity
*   vector to determine the direction of the ray although the masses are
*   slowed down during collision and therefore the velocity will no more
*   be suited for a successful collision test.  It's usually better to use
*   the impact based method instead if no function is available.  Apart
*   from that it won't successfully find the intersection in all situations
*   like the function based method.
*
*   =======================================================================
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Oct 2002 : added object intersection based method
*
******************************************************************************/

void Mechsim_Force_Collide_Environment(int Index, VECTOR Accel,VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_ENVIRONMENT *Env)
{
  DBL Potential, Force, Fn_Val, Len;
  VECTOR pot_force, Grad;
  VECTOR FFrict, damp_force;

  if (Env->Function != NULL)  // ======== if function is given use it to calculate gradient ===========
  {
    FunctionCode *f = POVFPU_GetFunction(*Env->Function);

    // ---- collision test ----
    POVFPU_SetLocal(X, Pos_Array[Index][X]);
    POVFPU_SetLocal(Y, Pos_Array[Index][Y]);
    POVFPU_SetLocal(Z, Pos_Array[Index][Z]);
    if (f->parameter_cnt > 3) POVFPU_SetLocal(T, Time); // additional time parameter

    Fn_Val = POVFPU_Run(*Env->Function);

    if (Fn_Val < MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess)
    {
      Function_Gradient( *Env->Function, Pos_Array[Index], Grad);
      VNormalizeEq(Grad);

      // --- potential force ---
      Potential = max(0.0, MechsimOptions.Data.Masses[Index].Radius - Fn_Val);
      VScale(pot_force, Grad, Potential*Env->Stiffness / MechsimOptions.Data.Masses[Index].Mass);

      // --- damping/friction force ---
      if (Env->Friction > 0.0)
      {
        // friction already slightly above the surface
        Force = (MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess - Fn_Val)*Env->Stiffness;
        Calc_Friction(FFrict, Vel_Array[Index], Env->Friction, Grad, Force);
      }
      else Make_Vector(FFrict, 0.0, 0.0, 0.0);

      damp_force[X] = -(fabs(Grad[X])*Potential*Env->Damping*Vel_Array[Index][X] + FFrict[X])/MechsimOptions.Data.Masses[Index].Mass;
      damp_force[Y] = -(fabs(Grad[Y])*Potential*Env->Damping*Vel_Array[Index][Y] + FFrict[Y])/MechsimOptions.Data.Masses[Index].Mass;
      damp_force[Z] = -(fabs(Grad[Z])*Potential*Env->Damping*Vel_Array[Index][Z] + FFrict[Z])/MechsimOptions.Data.Masses[Index].Mass;

      VAddEq(pot_force, damp_force);
      VAddEq(Accel, pot_force);

      Increase_Counter(stats[MechSim_Environment_Collisions]);
    }
  }
  else if (Env->Object != NULL) // ======== if only object is given use intersection test ===========
  {
    INTERSECTION Intersect;
    RAY Ray;
    VECTOR Dir, Norm;

    Initialize_Ray_Containers(&Ray);

    VNormalize(Dir, Vel_Array[Index]);
    Assign_Vector(Ray.Direction, Dir);
    Assign_Vector(Ray.Initial, Pos_Array[Index]);

    if(Intersection(&Intersect, Env->Object, &Ray))
    {
      Normal(Norm, Intersect.Object, &Intersect);      // get the normal
      VNormalizeEq(Norm);
      VSub(Dir, Intersect.IPoint, Pos_Array[Index]);   // approximate the distance (assuming flat surface)
      VDot(Len, Norm, Dir);

      // see if we need to invert the normal
      if (Len<0)
      {
        VScaleEq(Norm, -1);
        Len = -Len;
      }

      if (Len < MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess)
      {
        // --- potential force ---
        Potential = min(0.0, Len - MechsimOptions.Data.Masses[Index].Radius);
        VScale(pot_force, Norm, Potential*Env->Stiffness / MechsimOptions.Data.Masses[Index].Mass);

        // --- damping/friction force ---
        if (Env->Friction > 0.0)
        {
          // friction already slightly above the surface
          Force = (MechsimOptions.Data.Masses[Index].Radius*Env->Friction_Excess - Len)*Env->Stiffness;
          Calc_Friction(FFrict, Vel_Array[Index], Env->Friction, Norm, Force);
        }
        else Make_Vector(FFrict, 0.0, 0.0, 0.0);

        damp_force[X] = -(fabs(Norm[X])*Potential*Env->Damping*Vel_Array[Index][X] + FFrict[X])/MechsimOptions.Data.Masses[Index].Mass;
        damp_force[Y] = -(fabs(Norm[Y])*Potential*Env->Damping*Vel_Array[Index][Y] + FFrict[Y])/MechsimOptions.Data.Masses[Index].Mass;
        damp_force[Z] = -(fabs(Norm[Z])*Potential*Env->Damping*Vel_Array[Index][Z] + FFrict[Z])/MechsimOptions.Data.Masses[Index].Mass;

        VAddEq(pot_force, damp_force);
        VAddEq(Accel, pot_force);

        Increase_Counter(stats[MechSim_Environment_Collisions]);
      }
    }

  }

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Collide_Dynamic
*
* INPUT
*
*   Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from point mass collisions
*
*   For all mass pairs in collision elastic and damping force are calculated
*   and added to the 'Accel' vector of both masses.
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Sep 2002 : Modification for grouping masses for faster simulation
*       Jan 2003 : Added connection-connection collisions, a lot of cleanup
*       Sep 2004 : Unified Gradient and Force version
*
******************************************************************************/

void Mechsim_Collide_Dynamic(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	int i, j, k;
	int Grp1, Grp2;
	int Idx1, Idx2;
	int Start_Group, End_Group;
	DBL Unit;

	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_BBOX) 
		calc_mass_groups_bounding_box(Pos_Array, Bounding_Data);
	  
	/* ==================================================================================  */
	/* mass-mass collisions                                                                */
	/* ==================================================================================  */
	if (MechsimOptions.Calc_Collision != MECHSIM_COLLISION_NONE)
	{
		/* === plain collision testing (groups) === */
		if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_NO)
		{
			if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) Start_Group=0;
			else Start_Group=1;

			for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
			{
				if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) End_Group=Grp1;
				else End_Group=Grp1-1;

				for (Grp2=0; Grp2<=End_Group; Grp2++)
				{
					/*
						This double loop accesses each combination of
						two groups that need to be tested against each other once.
						If (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL)
						Grp1 and Grp2 can be identical (for testing collisions within each group)
					*/
					for (i=0; i<MechsimOptions.Data.Groups[Grp1].mass_count; i++)
						for (j=0; j<MechsimOptions.Data.Groups[Grp2].mass_count; j++)
						{
							/*
								Indices of the masses in conventional numbering,
								'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
								'MechsimOptions.Data.Masses[Idx1].Mass' are identical
								so this won't be necessary for the properties, but
								Vel_Array, Pos_Array and Accel need it.
							*/
							Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
							Idx2 = MechsimOptions.Data.Groups[Grp2].Masses[j]->Index;

							if (Idx1!=Idx2) /* avoid mass colliding with itself */
							{
								Mechsim_Collide_Mass_Mass(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
							}
						} /* mass for loop */
				} /* group for loop */
			}
		}

		/* === new bounding group version === */
		else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_BBOX)
		{
			if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) Start_Group=0;
			else Start_Group=1;

			for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
			{
				if (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL) End_Group=Grp1;
				else End_Group=Grp1-1;

				for (Grp2=0; Grp2<=End_Group; Grp2++)
				{
					/* We just check if the two groups intersect. */
					if (intersect_box_box(Bounding_Data->Mass_Groups_BBox_Array[Grp1], 
																Bounding_Data->Mass_Groups_BBox_Array[Grp2]))
					{
						calc_mass_group_intersection(Pos_Array, 
																				 Bounding_Data->Mass_Groups_BBox_Array[Grp1], Grp2, Bounding_Data);
						for (i=0; i<MechsimOptions.Data.Groups[Grp1].mass_count; i++)
						{
							/*
								Indices of the masses in conventional numbering,
								'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
								'MechsimOptions.Data.Masses[Idx1].Mass' are identical
								so this won't be necessary for the properties, but
								Vel_Array, Pos_Array and Accel need it.
							*/
							Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
							if (intersect_sphere_box(Bounding_Data->Mass_Groups_BBox_Array[Grp2], 
																			 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius))
							{
								for (j = 0; j < Bounding_Data->intersect_list_index; j++)
								{
									Idx2 = MechsimOptions.Data.Groups[Grp2].Masses[j]->Index;
									if (Idx1!=Idx2) /* avoid mass colliding with itself */
									{
										Mechsim_Collide_Mass_Mass(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
									}
								}
							}
						} /* mass for loop */
					}
				} /* group for loop */
			}
		}
		/* === hash table version === */
		else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
		{
			Unit = calc_mass_unit_size();
			Make_Vector(Bounding_Data->Unit, Unit, Unit, Unit);
               
			calc_masses_bounding_hashtable(Pos_Array, Bounding_Data);

			for (Idx1 = 0; Idx1 < MechsimOptions.Data.mass_count; Idx1++)
			/* loop through all masses */
			{
				intersect_sphere_bounding_hashtable(Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius, Bounding_Data);
				for (k = 0; k < Bounding_Data->intersect_list_index; k++)
				{
					Idx2 = Bounding_Data->intersect_list[k];
					if (Idx1 != Idx2)
					{
						Mechsim_Collide_Mass_Mass(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
					}
				} /* mass for loop */			
			} /* mass for loop */
		}
	}

	/* ==================================================================================  */
	/* mass-face collisions                                                                */
	/* ==================================================================================  */
	if ((MechsimOptions.Calc_Collision_Faces != MECHSIM_COLLISION_NONE) && (MechsimOptions.Data.faces_count>0))
	{
		/* === plain collision testing (groups) === */
		if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_NO)
		{
			for (Grp1=0; Grp1<MechsimOptions.Data.group_count; Grp1++)
				for (Grp2=0; Grp2<MechsimOptions.Data.group_count; Grp2++)
					if (
							(MechsimOptions.Calc_Collision_Faces == MECHSIM_COLLISION_ALL) ||
							(Grp1 != Grp2)
						 )
					{
						/*
							This double loop accesses each combination of
							two groups that need to be tested against each other *TWICE*
							(in contrast to the mass mass collision.  One time for testing the
							faces of Grp1 with the masses of Grp2, the other vica verse.
							If (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL)
							Grp1 and Grp2 can be identical (for testing collisions within each group)
						*/
						for (i=0; i<MechsimOptions.Data.Groups[Grp1].mass_count; i++)
							for (j=0; j<MechsimOptions.Data.Groups[Grp2].faces_count; j++)
							{
								/*
									Indices of the masses and faces in conventional numbering,
									'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
									'MechsimOptions.Data.Masses[Idx1].Mass' are identical
									so this won't be necessary for the properties, but
									Vel_Array, Pos_Array and Accel need it.
								*/
								Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
								Idx2 = MechsimOptions.Data.Groups[Grp2].Faces[j]->Index;

								if (
										(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx1) &&
										(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx2) &&
										(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx3) 
										/* avoid testing against a face the mass i belongs to */
									 )
								{
									Mechsim_Collide_Mass_Face(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
								}
							} /* mass for loop */
					} /* group for loop */
		}
		/* === new bounding group version === */
		else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_BBOX)
		{
			calc_face_groups_bounding_box(Pos_Array, Bounding_Data);
    
			for (Grp1=0; Grp1<MechsimOptions.Data.group_count; Grp1++)
				for (Grp2=0; Grp2<MechsimOptions.Data.group_count; Grp2++)

					if (
							(MechsimOptions.Calc_Collision_Faces == MECHSIM_COLLISION_ALL) ||
							((Grp1 != Grp2) &&
							 intersect_box_box(Bounding_Data->Mass_Groups_BBox_Array[Grp1], 
																 Bounding_Data->Face_Groups_BBox_Array[Grp2])
							)
						 )
					{
						calc_face_group_intersection(Bounding_Data->Mass_Groups_BBox_Array[Grp1], Grp2, Bounding_Data);
						/*
							This double loop accesses each combination of
							two groups that need to be tested against each other *TWICE*
							(in contrast to the mass mass collision.  One time for testing the
							faces of Grp1 with the masses of Grp2, the other vica verse.
							If (MechsimOptions.Calc_Collision == MECHSIM_COLLISION_ALL)
							Grp1 and Grp2 can be identical (for testing collisions within each group)
						*/
						for (i = 0; i < MechsimOptions.Data.Groups[Grp1].mass_count; i++)
						{
							Idx1 = MechsimOptions.Data.Groups[Grp1].Masses[i]->Index;
							/* We just check if the mass intersects the second group. */
							if (intersect_sphere_box(Bounding_Data->Face_Groups_BBox_Array[Grp2], 
																			 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius))
							{
								for (j = 0; j < Bounding_Data->intersect_list_index; j++)
								{
									/*
										Indices of the masses and faces in conventional numbering,
										'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
										'MechsimOptions.Data.Masses[Idx1].Mass' are identical
										so this won't be necessary for the properties, but
										Vel_Array, Pos_Array and Accel need it.
									*/
									Idx2 = Bounding_Data->intersect_list[j];

									if (
											(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx1) &&
											(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx2) &&
											(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx3) && /* avoid testing against a face the mass i belongs to */
											intersect_sphere_box(Bounding_Data->Faces_BBox_Array[Idx2], 
																					 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius)
											/* quick check if mass intersects face */
										 )
									{
										Mechsim_Collide_Mass_Face(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
									}
								}
							}
						} 
					}
		}	
		/* === hash table version === */
		else if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
		{
			calc_faces_bounding_box(Pos_Array, Bounding_Data);
			calc_faces_bounding_hashtable(Bounding_Data);

			for (Idx1 = 0; Idx1 < MechsimOptions.Data.mass_count; Idx1++)
			/* loop through all masses */
			{
				intersect_sphere_bounding_hashtable(Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius, Bounding_Data);
				for (k = 0; k < Bounding_Data->intersect_list_index; k++)
				{
					Idx2 = Bounding_Data->intersect_list[k];
					if (
							(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx1) &&
							(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx2) &&
							(Idx1 != MechsimOptions.Data.Faces[Idx2].Idx3) && /* avoid testing against a face the mass i belongs to */
							(
							 (MechsimOptions.Calc_Collision_Faces == MECHSIM_COLLISION_ALL) ||
							 (MechsimOptions.Data.Faces[Idx2].Group != MechsimOptions.Data.Masses[Idx1].Group)
							) &&
							/* and make sure mass and face are in different groups */
							intersect_sphere_box(Bounding_Data->Faces_BBox_Array[Idx2], 
																	 Pos_Array[Idx1], MechsimOptions.Data.Masses[Idx1].Radius)
						 )
					{
						Mechsim_Collide_Mass_Face(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
					}
				} /* face for loop */			
			} /* mass for loop */
		}
	}

	/* ==================================================================================  */
	/* connection-connection collisions                                                    */
	/* ==================================================================================  */
	if ((MechsimOptions.Calc_Collision_Connections != MECHSIM_COLLISION_NONE) && (MechsimOptions.Data.connect_count>0))
	{
		/* === plain collision testing (groups) === */
		if (MechsimOptions.Bounding < MECHSIM_FACE_BOUNDING_BBOX)
		{
			if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) Start_Group=0;
			else Start_Group=1;

			for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
			{
				if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) End_Group=Grp1;
				else End_Group=Grp1-1;

				for (Grp2=0; Grp2<=End_Group; Grp2++)
				{
					/*
						This double loop accesses each combination of
						two groups that need to be tested against each other once (just like in
						mass-mass-collisions).
						If (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL)
						Grp1 and Grp2 can be identical (for testing collisions within each group)
					*/
					for (i=0; i<MechsimOptions.Data.Groups[Grp1].connect_count; i++)
						for (j=0; j<MechsimOptions.Data.Groups[Grp2].connect_count; j++)
						{
							/*
								Indices of the connections in conventional numbering,
								see mass-mass-collisions for more details.
							*/
							Idx1 = MechsimOptions.Data.Groups[Grp1].Connects[i]->Index;
							Idx2 = MechsimOptions.Data.Groups[Grp2].Connects[j]->Index;

							if ((Idx1!=Idx2) && /* avoid connection colliding with itself */
									(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
									 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1) &&
									(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
									 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
									(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
									 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
									(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
									 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1))
								/* and also do not test connections with common masses */
							{
								Mechsim_Collide_Connection_Connection(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
							}
						} /* connection for loop */
				} /* group for loop */
			}
		}
		/* === new bounding group version === */
		else
		{
			calc_connect_groups_bounding_box(Pos_Array, Bounding_Data);
			if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) Start_Group=0;
			else Start_Group=1;

			for (Grp1=Start_Group; Grp1<MechsimOptions.Data.group_count; Grp1++)
			{
				if (MechsimOptions.Calc_Collision_Connections == MECHSIM_COLLISION_ALL) End_Group=Grp1;
				else End_Group=Grp1-1;

				for (Grp2=0; Grp2<=End_Group; Grp2++)
				{
					/* We just check if the two groups intersect. */
					if (intersect_box_box(Bounding_Data->Connect_Groups_BBox_Array[Grp1], 
																Bounding_Data->Connect_Groups_BBox_Array[Grp2]))
					{
						calc_connect_group_intersection(Pos_Array, Bounding_Data->Connect_Groups_BBox_Array[Grp1], Grp2, Bounding_Data);
						for (i = 0; i < MechsimOptions.Data.Groups[Grp1].connect_count; i++)
						{
							Idx1 = MechsimOptions.Data.Groups[Grp1].Connects[i]->Index;
							/* We just check if the connection intersects the second group. */
							if (intersect_box_box(Bounding_Data->Connect_Groups_BBox_Array[Grp2], 
							                      Bounding_Data->Connects_BBox_Array[Idx1]))
							{
								for (j = 0; j < Bounding_Data->intersect_list_index; j++)
								{
									/*
										Indices of the masses and faces in conventional numbering,
										'MechsimOptions.Data.Groups[Grp1].Masses[i]->Mass' and
										'MechsimOptions.Data.Masses[Idx1].Mass' are identical
										so this won't be necessary for the properties, but
										Vel_Array, Pos_Array and Accel need it.
									*/
									Idx2 = Bounding_Data->intersect_list[j];

									if ((Idx1!=Idx2) && /* avoid connection colliding with itself */
											(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
											 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1) &&
											(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
											 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
											(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx1 !=
											 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx2) &&
											(MechsimOptions.Data.Groups[Grp1].Connects[i]->Idx2 !=
											 MechsimOptions.Data.Groups[Grp2].Connects[j]->Idx1) &&
											/* and also do not test connections with common masses */
											intersect_box_box(Bounding_Data->Connects_BBox_Array[Idx1],
																				Bounding_Data->Connects_BBox_Array[Idx2]))
									{
										Mechsim_Collide_Connection_Connection(Accel, Pos_Array, Vel_Array, Idx1, Idx2);
									}
								}
							} /* connection for loop */
						}
					}
				} /* group for loop */
			}
		}
	}
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Collide_Mass_Mass
*
* INPUT
*
*   Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
*   Idx1, Idx2 - index of the masses
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from mass-mass collisions
*
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Sep 2004 : moved to separate function by Daniel Jungmann
*
******************************************************************************/
void Mechsim_Collide_Mass_Mass(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2)
{
	DBL Dist, lAxis;
	VECTOR Axis, Vel;
	VECTOR damp_force;
	VECTOR pot_force, FFrict;
	DBL Force;
	DBL Potential;
	
	VSub(Axis, Pos_Array[Idx1], Pos_Array[Idx2]);
	VLength(lAxis, Axis);
	Dist = MechsimOptions.Data.Masses[Idx1].Radius+MechsimOptions.Data.Masses[Idx2].Radius;

	if (lAxis < Dist*MechsimOptions.Collision_Friction_Excess)
	{
		VInverseScaleEq(Axis, lAxis);

		// --- potential force ---
		Potential = max(0.0, Dist-lAxis);
		VScale(pot_force, Axis,  Potential*MechsimOptions.Collision_Stiffness);

		// Force version (with damping and mass)
		if (Vel_Array != NULL)
		{
			// --- damping/friction force ---
			VSub(Vel, Vel_Array[Idx1], Vel_Array[Idx2]); // relative velocity

			if (MechsimOptions.Collision_Friction > 0.0)
			{
				// friction already slightly above the surface
				Force = (Dist*MechsimOptions.Collision_Friction_Excess - lAxis)*MechsimOptions.Collision_Stiffness;
				Calc_Friction(FFrict, Vel, MechsimOptions.Collision_Friction, Axis, Force);
			}
			else Make_Vector(FFrict, 0.0, 0.0, 0.0);

			damp_force[X] = -(fabs(Axis[X])*Potential*MechsimOptions.Collision_Damping*Vel[X] + FFrict[X]);
			damp_force[Y] = -(fabs(Axis[Y])*Potential*MechsimOptions.Collision_Damping*Vel[Y] + FFrict[Y]);
			damp_force[Z] = -(fabs(Axis[Z])*Potential*MechsimOptions.Collision_Damping*Vel[Z] + FFrict[Z]);

			VAddEq(pot_force, damp_force);

			VAddScaledEq(Accel[Idx1],  1.0/MechsimOptions.Data.Masses[Idx1].Mass, pot_force);
			VAddScaledEq(Accel[Idx2], -1.0/MechsimOptions.Data.Masses[Idx1].Mass, pot_force);
		}
		// Gradient version
		else
		{
			VAddEq(Accel[Idx1], pot_force);
			VSubEq(Accel[Idx2], pot_force);
		}

		Increase_Counter(stats[MechSim_Mass_Collisions]);
	}
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Collide_Mass_Face
*
* INPUT
*
*   Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
*   Idx1, Idx2 - index of the mass and face
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from mass-face collisions
*
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Sep 2004 : moved to separate function by Daniel Jungmann
*
******************************************************************************/
void Mechsim_Collide_Mass_Face(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2)
{
	VECTOR Grad;
	VECTOR CPoint;
	VECTOR damp_force;
	VECTOR pot_force, FFrict;
	VECTOR Vel;
	DBL Force;
	DBL Potential;
	DBL Fn_Val, Ratio;
	DBL Weight1, Weight2, Weight3;
	int Side;
	
	// ---- collision test ----
	// also calculates contact point and gradient
	Side = func_triangle(Pos_Array[Idx1], CPoint, Grad,
											 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx1],
											 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx2],
											 Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx3], &Ratio);

	VLength(Fn_Val, Grad);

	if (Fn_Val < MechsimOptions.Data.Masses[Idx1].Radius*MechsimOptions.Collision_Friction_Excess)
	{
		VInverseScaleEq(Grad, Fn_Val);

		// --- now calculate distribution of reaction force between triangle vertices ---

		switch (Side)
		{
			// if outside, the triangle function has already returned the nearest side
			case 1: /* P1-P2 */
				Weight1 = 1.0-Ratio;
				Weight2 = Ratio;
				Weight3 = 0.0;
				break;
			case 2: /* P2-P3 */
				Weight2 = 1.0-Ratio;
				Weight3 = Ratio;
				Weight1 = 0.0;
				break;
			case 3: /* P3-P1 */
				Weight3 = 1.0-Ratio;
				Weight1 = Ratio;
				Weight2 = 0.0;
				break;
			default:
				// if inside triangle, calculate distribution between the three vertices
				Triangle_Distribute(CPoint,
														Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx1],
														Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx2],
														Pos_Array[MechsimOptions.Data.Faces[Idx2].Idx3],
														&Weight1, &Weight2, &Weight3);
				break;
		}

		// --- potential force ---
		Potential = max(0.0, MechsimOptions.Data.Masses[Idx1].Radius - Fn_Val);
		VScale(pot_force, Grad, Potential*MechsimOptions.Collision_Stiffness);

		// Force version (with damping and mass)
		if (Vel_Array != NULL)
		{
			// --- damping/friction force ---
			Vel[X] = Vel_Array[Idx1][X] - (1.0/3.0)*(Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx1][X]+
																							 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx2][X]+
																							 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx3][X]);
			Vel[Y] = Vel_Array[Idx1][Y] - (1.0/3.0)*(Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx1][Y]+
																							 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx2][Y]+
																							 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx3][Y]);
			Vel[Z] = Vel_Array[Idx1][Z] - (1.0/3.0)*(Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx1][Z]+
																							 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx2][Z]+
																							 Vel_Array[MechsimOptions.Data.Faces[Idx2].Idx3][Z]); // relative velocity

			if (MechsimOptions.Collision_Friction > 0.0)
			{
				// friction already slightly above the surface
				Force = (MechsimOptions.Data.Masses[Idx1].Radius*MechsimOptions.Collision_Friction_Excess - Fn_Val)*MechsimOptions.Collision_Stiffness;
				Calc_Friction(FFrict, Vel, MechsimOptions.Collision_Friction, Grad, Force);
			}
			else Make_Vector(FFrict, 0.0, 0.0, 0.0);

			damp_force[X] = -(fabs(Grad[X])*Potential*MechsimOptions.Collision_Damping*Vel[X] + FFrict[X]);
			damp_force[Y] = -(fabs(Grad[Y])*Potential*MechsimOptions.Collision_Damping*Vel[Y] + FFrict[Y]);
			damp_force[Z] = -(fabs(Grad[Z])*Potential*MechsimOptions.Collision_Damping*Vel[Z] + FFrict[Z]);

			VAddEq(pot_force, damp_force); // apply to mass
			VAddScaledEq(Accel[Idx1], 1.0/MechsimOptions.Data.Masses[Idx1].Mass, pot_force);

			// apply forces to the vertices
			VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx1],
									 -Weight1/MechsimOptions.Data.Masses[MechsimOptions.Data.Faces[Idx2].Idx1].Mass,
									 pot_force);
			VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx2],
									 -Weight2/MechsimOptions.Data.Masses[MechsimOptions.Data.Faces[Idx2].Idx2].Mass,
									 pot_force);
			VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx3],
									 -Weight3/MechsimOptions.Data.Masses[MechsimOptions.Data.Faces[Idx2].Idx3].Mass,
									 pot_force);

		}
		// Gradient version
		else
		{
			VAddEq(Accel[Idx1], pot_force);

			// apply forces to the vertices
			VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx1], -Weight1, pot_force);
			VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx2], -Weight2, pot_force);
			VAddScaledEq(Accel[MechsimOptions.Data.Faces[Idx2].Idx3], -Weight3, pot_force);
		}

		Increase_Counter(stats[MechSim_Face_Collisions]);

  }
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Collide_Mass_Face
*
* INPUT
*
*   Pos_Array, Vel_Array - state of the masses (Vel_Array == NULL - gradient version)
*   Idx1, Idx2 - index of the connections
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from connection-connection collisions
*
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Sep 2004 : moved to separate function by Daniel Jungmann
*
******************************************************************************/
void Mechsim_Collide_Connection_Connection(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array, int Idx1, int Idx2)
{
	VECTOR C, D, R1, R2;
	VECTOR damp_force;
	VECTOR pot_force, FFrict;
	VECTOR Vel;
	DBL Dist, lAxis;
	DBL Force;
	DBL Potential;
	DBL Fact, afact, bfact;
	int Dir1, Dir2, Dir3;

	/*
		for connection-connection collisions we need to find the distance
		between two straight lines defined by the connections and have to
		test if this smallest distance is less than the sum of the radii
		and if it occurs between the end points of the connections.  The
		problem can be described by the (vector) equation:

			P1 + alpha*(P2-P1) + gamma*D = Q1 + beta*(Q2-Q1)

		where D = vnormalize(vcross((P2-P1), (Q2-Q1))).

		gamma is the distance between the lines, alpha and beta have to
		be between 0 and 1.  Further symbols used in the code:

			R1 = (P2-P1)
			R2 = (Q2-Q1)
			C = (Q1-P1)
	*/

	VSub(R1, Pos_Array[MechsimOptions.Data.Connects[Idx1].Idx2],
			     Pos_Array[MechsimOptions.Data.Connects[Idx1].Idx1] );
	VSub(R2, Pos_Array[MechsimOptions.Data.Connects[Idx2].Idx2],
			     Pos_Array[MechsimOptions.Data.Connects[Idx2].Idx1] );
	VCross(D, R1, R2);

	VLength(Dist, D);

	if (Dist>MECHSIM_EPSILON)  /* the connection lines must not be collinear */
	{
		VInverseScaleEq(D, Dist);

		if (fabs(R1[X])>fabs(R1[Y])) /* Sort the Coordinates to avoid div by zero */
		{
			if (fabs(R1[X])>fabs(R1[Z])) { Dir1 = X; Dir2 = Y; Dir3 = Z; }
			else { Dir1 = Z; Dir2 = Y; Dir3 = X; }
		}
		else
		{
			if (fabs(R1[Y])>fabs(R1[Z])) { Dir1 = Y; Dir2 = X; Dir3 = Z; }
			else { Dir1 = Z; Dir2 = Y; Dir3 = X; }
		}

		if (fabs(R1[Dir1])>MECHSIM_EPSILON)
		{
			Fact = (-R2[Dir2] + R2[Dir1]*(R1[Dir2]/R1[Dir1]));
			if (Fact > MECHSIM_EPSILON)
			{
				Fact = (-R2[Dir3] + R2[Dir1]*(R1[Dir3]/R1[Dir1])) / Fact;

				lAxis = (D[Dir3] - D[Dir1]*(R1[Dir3]/R1[Dir1]) - (D[Dir2]-D[Dir1]*(R1[Dir2]/R1[Dir1]))*Fact);

				if (lAxis > MECHSIM_EPSILON)
				{
					VSub(C, Pos_Array[MechsimOptions.Data.Connects[Idx2].Idx1],
							    Pos_Array[MechsimOptions.Data.Connects[Idx1].Idx1]);
					lAxis = (C[Dir3] - C[Dir1]*(R1[Dir3]/R1[Dir1]) - (C[Dir2]-C[Dir1]*(R1[Dir2]/R1[Dir1]))*Fact) / lAxis;
				}
				else return;
			}
			else return;

			Dist = (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx1].Radius+
							MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx2].Radius+
							MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx1].Radius+
							MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx2].Radius)*0.5;

		}
		else return;

		if ((fabs(lAxis) < Dist*MechsimOptions.Collision_Friction_Excess) &&
				(fabs(lAxis) > MECHSIM_EPSILON))
		{
			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]));

			if ((bfact < 1.0) && (bfact > 0.0))
			{
				afact = (C[Dir1] - lAxis*D[Dir1] + bfact*R2[Dir1]) / R1[Dir1];

				if ((afact < 1.0) && (afact > 0.0))
				{

					// --- potential force ---
					lAxis = fabs(lAxis);
					Potential = max(0.0, Dist - lAxis);

					VScale(pot_force, D, Potential*MechsimOptions.Collision_Stiffness);

					// Force version (with damping and mass)
					if (Vel_Array != NULL)
					{
						// --- damping/friction force ---
						Vel[X] = ((Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx1][X] + Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx2][X]) -
											(Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx1][X] + Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx2][X]))/2.0;
						Vel[Y] = ((Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx1][Y] + Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx2][Y]) -
											(Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx1][Y] + Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx2][Y]))/2.0;
						Vel[Z] = ((Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx1][Z] + Vel_Array[MechsimOptions.Data.Connects[Idx1].Idx2][Z]) -
											(Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx1][Z] + Vel_Array[MechsimOptions.Data.Connects[Idx2].Idx2][Z]))/2.0;
						// relative velocity

						if (MechsimOptions.Collision_Friction > 0.0)
						{
							// friction already slightly above the surface
							Force = (Dist*MechsimOptions.Collision_Friction_Excess - lAxis)*MechsimOptions.Collision_Stiffness;
							Calc_Friction(FFrict, Vel, MechsimOptions.Collision_Friction, D, Force);
						}
						else Make_Vector(FFrict, 0.0, 0.0, 0.0);

						damp_force[X] = -(fabs(D[X])*Potential*MechsimOptions.Collision_Damping*Vel[X] + FFrict[X]);
						damp_force[Y] = -(fabs(D[Y])*Potential*MechsimOptions.Collision_Damping*Vel[Y] + FFrict[Y]);
						damp_force[Z] = -(fabs(D[Z])*Potential*MechsimOptions.Collision_Damping*Vel[Z] + FFrict[Z]);

						VAddEq(pot_force, damp_force); // apply to masses

						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx1],
												 (1.0-afact)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx1].Mass, pot_force);
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx2],
												 afact/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx1].Idx2].Mass, pot_force);
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx1],
												 -(1.0-bfact)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx1].Mass, pot_force);
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx2],
												 -bfact/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx2].Idx2].Mass, pot_force);
					}
					// Gradient version
					else
					{
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx1],  (1.0-afact), pot_force);
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx1].Idx2],        afact, pot_force);
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx1], -(1.0-bfact), pot_force);
						VAddScaledEq(Accel[MechsimOptions.Data.Connects[Idx2].Idx2],       -bfact, pot_force);
					}

					Increase_Counter(stats[MechSim_Connection_Collisions]);
				}
			}
		}
	}
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradient_Connect
*
* INPUT
*
*   Pos_Array - state of the masses
*
* OUTPUT
*
*   calculated gradients of the potential fields are added to 'Gradient'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Variation of Mechsim_Force_Connect() for use with gradient
*   descent method.  No damping forces.
*
* CHANGES
*
*   --- Sep 2002 : Creation
*
******************************************************************************/

void Mechsim_Gradient_Connect(VECTOR *Gradient, VECTOR *Pos_Array)
{
  int i;
  DBL lAxis;
  VECTOR Axis;
  VECTOR pot_force;

  for (i=0; i<MechsimOptions.Data.connect_count; i++)
  {
    VSub(Axis, Pos_Array[MechsimOptions.Data.Connects[i].Idx1],
               Pos_Array[MechsimOptions.Data.Connects[i].Idx2] );
    VLength(lAxis, Axis);

    if (lAxis>0)  // otherwise we can't conclude the direction
    {
      // --- potential force ---
      VScale(pot_force, Axis, -((lAxis-MechsimOptions.Data.Connects[i].Length)/lAxis)*MechsimOptions.Data.Connects[i].Stiffness);

      VAddEq(Gradient[MechsimOptions.Data.Connects[i].Idx1], pot_force);
      VSubEq(Gradient[MechsimOptions.Data.Connects[i].Idx2], pot_force);

    }
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Force_Connect
*
* INPUT
*
*   Pos_Array, Vel_Array - state of the masses
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from connections
*
*   Looping through the connection array all connected mass pairs are
*   handled and accelerations added to the 'Accel' vector of both masses.
*
* CHANGES
*
*   --- Aug 2002 : Creation
*
******************************************************************************/

void Mechsim_Force_Connect(VECTOR *Accel, VECTOR *Pos_Array, VECTOR *Vel_Array)
{
  int i;
  DBL lAxis, Force_Val;
  VECTOR Axis;
  VECTOR pot_force1, pot_force2;
  DBL damp_fact;

  for (i=0; i<MechsimOptions.Data.connect_count; i++)
  {
    VSub(Axis, Pos_Array[MechsimOptions.Data.Connects[i].Idx1],
               Pos_Array[MechsimOptions.Data.Connects[i].Idx2] );
    VLength(lAxis, Axis);

    if (lAxis>0)  // otherwise we can't conclude the direction
    {
      // --- potential force ---
			Force_Val = (lAxis-MechsimOptions.Data.Connects[i].Length)*MechsimOptions.Data.Connects[i].Stiffness;

      // --- damping force ---

      if (MechsimOptions.Data.Connects[i].Damping > 0.0)
      {
        damp_fact =   // (x*vx + y*vy + z*vz)/ (x^2 + y^2 + z^2)
          (
           (Pos_Array[MechsimOptions.Data.Connects[i].Idx1][X]-Pos_Array[MechsimOptions.Data.Connects[i].Idx2][X])*
           (Vel_Array[MechsimOptions.Data.Connects[i].Idx1][X]-Vel_Array[MechsimOptions.Data.Connects[i].Idx2][X])
           +
           (Pos_Array[MechsimOptions.Data.Connects[i].Idx1][Y]-Pos_Array[MechsimOptions.Data.Connects[i].Idx2][Y])*
           (Vel_Array[MechsimOptions.Data.Connects[i].Idx1][Y]-Vel_Array[MechsimOptions.Data.Connects[i].Idx2][Y])
           +
           (Pos_Array[MechsimOptions.Data.Connects[i].Idx1][Z]-Pos_Array[MechsimOptions.Data.Connects[i].Idx2][Z])*
           (Vel_Array[MechsimOptions.Data.Connects[i].Idx1][Z]-Vel_Array[MechsimOptions.Data.Connects[i].Idx2][Z])
          )/(lAxis*lAxis);

				Force_Val += damp_fact*MechsimOptions.Data.Connects[i].Damping;
      }

      VScale(pot_force1, Axis, -(Force_Val/lAxis)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[i].Idx1].Mass);
			VScale(pot_force2, Axis,  (Force_Val/lAxis)/MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[i].Idx2].Mass);

      VAddEq(Accel[MechsimOptions.Data.Connects[i].Idx1], pot_force1);
      VAddEq(Accel[MechsimOptions.Data.Connects[i].Idx2], pot_force2);

    }
  }
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Connect_Evaluate
*
* INPUT
*
*   Idx - number of connection to evaluate
*
* OUTPUT
*
* RETURNS
*
*   The force of the connection
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the force resulting from a connection.
*
*   This is supposed to be called after the simulation is finished.
*
* CHANGES
*
*   --- Mar 2005 : Creation
*
******************************************************************************/

DBL Mechsim_Connect_Evaluate(int Idx)
{
  DBL lAxis, xForce;
  VECTOR Axis;
  DBL damp_fact;


	VSub(Axis, MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position,
			       MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position );
	VLength(lAxis, Axis);

	if (lAxis>0)  // otherwise we can't conclude the direction
  {
		// --- potential force ---
	  xForce = (lAxis-MechsimOptions.Data.Connects[Idx].Length)*MechsimOptions.Data.Connects[Idx].Stiffness;

		// --- damping force ---

		if (MechsimOptions.Data.Connects[Idx].Damping > 0.0)
    {
			damp_fact =   // (x*vx + y*vy + z*vz)/ (x^2 + y^2 + z^2)
				(
				 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position[X]-
					MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position[X])*
				 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Velocity[X]-
					MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Velocity[X])
				 +
				 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position[Y]-
					MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position[Y])*
				 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Velocity[Y]-
					MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Velocity[Y])
				 +
				 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Position[Z]-
					MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Position[Z])*
				 (MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx1].Velocity[Z]-
					MechsimOptions.Data.Masses[MechsimOptions.Data.Connects[Idx].Idx2].Velocity[Z])
				)/(lAxis*lAxis);


			xForce += damp_fact*MechsimOptions.Data.Connects[Idx].Damping;
		}
		
  }
	else xForce = 0.0;

	return xForce;
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradient_Viscoelastics
*
* INPUT
*
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated gradients of the potential fields are added to 'Gradient'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   viscoelastic elements are treated as elastic connections with
*   'Stiffness0'
*
* CHANGES
*
*   --- Feb 2005 : Creation
*
******************************************************************************/

void Mechsim_Gradient_Viscoelastics(VECTOR *Gradient, VECTOR *Pos_Array)
{
  int i;
  DBL lAxis;
  VECTOR Axis;
  VECTOR pot_force;

  for (i=0; i<MechsimOptions.Data.ve_count; i++)
  {
    VSub(Axis, Pos_Array[MechsimOptions.Data.Viscoelastics[i].Idx1],
               Pos_Array[MechsimOptions.Data.Viscoelastics[i].Idx2] );
    VLength(lAxis, Axis);

    if (lAxis>0)  // otherwise we can't conclude the direction
    {
      // --- potential force ---
      VScale(pot_force, Axis, -((lAxis-MechsimOptions.Data.Viscoelastics[i].Length)/lAxis)*MechsimOptions.Data.Viscoelastics[i].Stiffness0);

      VAddEq(Gradient[MechsimOptions.Data.Viscoelastics[i].Idx1], pot_force);
      VSubEq(Gradient[MechsimOptions.Data.Viscoelastics[i].Idx2], pot_force);

    }
  }
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Force_Viscoelastics
*
* INPUT
*
*   State_Array - state of the masses
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the forces resulting from viscoelastic elements
*
* CHANGES
*
*   --- Feb 2005 : Creation
*
******************************************************************************/

void Mechsim_Force_Viscoelastics(VECTOR *Accel, VECTOR *State_Array)
{
  int i, j;
  DBL lAxis, Delta, Force_Val;
  VECTOR Axis;
  VECTOR force, pot_force1, pot_force2;
  DBL damp_fact;

  for (i=0; i<MechsimOptions.Data.ve_count; i++)
  {
    VSub(Axis, State_Array[MechsimOptions.Data.Viscoelastics[i].Idx1],
               State_Array[MechsimOptions.Data.Viscoelastics[i].Idx2] );
    VLength(lAxis, Axis);

    if (lAxis>0)  // otherwise we can't conclude the direction
    {
			Delta = lAxis-MechsimOptions.Data.Viscoelastics[i].Length;

      // --- plain elastic force ---
			Force_Val = Delta*MechsimOptions.Data.Viscoelastics[i].Stiffness0;

      // --- element forces ---
			for (j=0; j<MechsimOptions.Data.Viscoelastics[i].element_count; j++)
			{
				Force_Val += (Delta - MechsimOptions.Data.Viscoelastics[i].Element[j].Y)*MechsimOptions.Data.Viscoelastics[i].Element[j].Stiffness;
			}

      VScale(force, Axis, -Force_Val/lAxis);

      VScale(pot_force1, force,  1.0/MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[i].Idx1].Mass);
      VScale(pot_force2, force, -1.0/MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[i].Idx2].Mass);

      VAddEq(Accel[MechsimOptions.Data.Viscoelastics[i].Idx1], pot_force1);
      VAddEq(Accel[MechsimOptions.Data.Viscoelastics[i].Idx2], pot_force2);

    }
  }
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Viscoelastic_Evaluate
*
* INPUT
*
*   Idx - number of viscoelastic connection to evaluate
*
* OUTPUT
*
* RETURNS
*
*   The force of the connection
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the force resulting from a viscoelastic connection.
*
*   This is supposed to be called after the simulation is finished.
*
* CHANGES
*
*   --- Mar 2005 : Creation
*
******************************************************************************/

DBL Mechsim_Viscoelastic_Evaluate(int Idx)
{
  int j;
  DBL lAxis, xForce, Delta;
  VECTOR Axis;
  DBL damp_fact;

	VSub(Axis, MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[Idx].Idx1].Position,
			       MechsimOptions.Data.Masses[MechsimOptions.Data.Viscoelastics[Idx].Idx2].Position );
	VLength(lAxis, Axis);

	if (lAxis>0)  // otherwise we can't conclude the direction
  {
		Delta = lAxis-MechsimOptions.Data.Viscoelastics[Idx].Length;

		// --- plain elastic force ---
	  xForce = Delta*MechsimOptions.Data.Viscoelastics[Idx].Stiffness0;

		// --- element forces ---
		for (j=0; j<MechsimOptions.Data.Viscoelastics[Idx].element_count; j++)
		{
			xForce += (Delta - MechsimOptions.Data.Viscoelastics[Idx].Element[j].Y)*MechsimOptions.Data.Viscoelastics[Idx].Element[j].Stiffness;
		}
		
  }
	else xForce = 0.0;

	return xForce;
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Integrate_Viscoelastics
*
* INPUT
*
*   State_Array - state of the masses
*
* OUTPUT
*
*   -
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the inner variables of all viscoelastic elements.
*   To be called once every simulation step.
*
*   The inner variables (see docs) follow the following ODE:
*
*     dy/dt = Stiffness/Damping * ( Delta - y )
*
*   with Delta = L-L0 (Length extension)
*
*   This function integrates this equation.  It uses first order
*   explicit method (Euler) with adjustable time stepping (via Accuracy)
*   but with linear elements this is probably non-critical.
*
* CHANGES
*
*   --- Feb 2005 : Creation
*
******************************************************************************/

void Mechsim_Integrate_Viscoelastics(VECTOR *State_Array)
{
  int i, j, k;
  DBL lAxis, Delta, Alpha, H;
  VECTOR Axis;

  for (i=0; i<MechsimOptions.Data.ve_count; i++)
  {
    VSub(Axis, State_Array[MechsimOptions.Data.Viscoelastics[i].Idx1],
               State_Array[MechsimOptions.Data.Viscoelastics[i].Idx2] );
    VLength(lAxis, Axis);

    if (lAxis>0)  // otherwise we can't conclude the direction
    {
			VInverseScaleEq(Axis, lAxis); // Normalize direction vector

			Delta = lAxis-MechsimOptions.Data.Viscoelastics[i].Length;
			H = MechsimOptions.Time_Step/(DBL)MechsimOptions.Data.Viscoelastics[i].Accuracy;

			for (j=0; j<MechsimOptions.Data.Viscoelastics[i].element_count; j++)
			{
				Alpha = MechsimOptions.Data.Viscoelastics[i].Element[j].Stiffness/MechsimOptions.Data.Viscoelastics[i].Element[j].Damping;

				for (k=0; k<MechsimOptions.Data.Viscoelastics[i].Accuracy; k++)
					MechsimOptions.Data.Viscoelastics[i].Element[j].Y += H * Alpha * ( Delta - MechsimOptions.Data.Viscoelastics[i].Element[j].Y );

				//Debug_Info("Viscoelastics %g %g\n", Delta, MechsimOptions.Data.Viscoelastics[i].Element[j].Y);
#if(MECHSIM_DEBUG == 1)
				//				Debug_Info("Viscoelastics %g %g\n", Delta, MechsimOptions.Data.Viscoelastics[i].Element[j].Y);
#endif

			}
    }
  }
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradient_Interactions
*
* INPUT
*
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Gradient descent version for the interactions
*
* CHANGES
*
*   --- Nov 2002 : Creation
*       Mar 2005 : modified for two separate function evaluations
*
******************************************************************************/

void Mechsim_Gradient_Interactions(VECTOR *Accel, VECTOR *Pos_Array)
{
  int i, j, k;
  DBL Fn_Val1, Fn_Val2, lAxis;
  VECTOR Axis;

  for (i=0; i<MechsimOptions.Data.mass_count; i++)
  {
    for (j=0; j<i; j++)
    /*
       This double loop accesses each combination of
       two different masses exactly once
    */
    {
      VSub(Axis, Pos_Array[i], Pos_Array[j] );
      VLength(lAxis, Axis);

			if (lAxis>0)  // otherwise we can't conclude the direction
			{
				VInverseScaleEq(Axis, lAxis); // Normalize direction vector

				for (k=0; k<MechsimOptions.Interactions.count; k++)
				{
					FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Interactions.Function[k]));

					// evaluate function for first mass
					POVFPU_SetLocal(X, Pos_Array[i][X]);
					POVFPU_SetLocal(Y, Pos_Array[i][Y]);
					POVFPU_SetLocal(Z, Pos_Array[i][Z]);
					if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
					// additional mass 1 parameter
					if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[i].Mass);
					// additional mass 2 parameter
					if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[j].Mass);

					Fn_Val1 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));

					// evaluate function for second mass
					POVFPU_SetLocal(X, Pos_Array[j][X]);
					POVFPU_SetLocal(Y, Pos_Array[j][Y]);
					POVFPU_SetLocal(Z, Pos_Array[j][Z]);
					if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
					// additional mass 1 parameter
					if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[j].Mass);
					// additional mass 2 parameter
					if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[i].Mass);

					Fn_Val2 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));

					if (fabs(Fn_Val1) > MECHSIM_EPSILON)
					{
						/* apply force to first mass */
						VAddScaledEq(Accel[i],  -Fn_Val1, Axis);
					}
					if (fabs(Fn_Val2) > MECHSIM_EPSILON)
					{
						/* apply force to second mass */
						VAddScaledEq(Accel[j], Fn_Val2, Axis);
					}
				}
      }
    }
  }

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Force_Interactions
*
* INPUT
*
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated accelerations are added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the function defined interactions between the masses
*
*   Interaction functions can have 4 parameters: the three spartial
*   coordinates and the distance between the masses.
*
* CHANGES
*
*   --- Nov 2002 : Creation
*       Mar 2005 : modified for two separate function evaluations
*
******************************************************************************/

void Mechsim_Force_Interactions(VECTOR *Accel, VECTOR *Pos_Array)
{
  int i, j, k;
  DBL Fn_Val1, Fn_Val2, lAxis;
  VECTOR Axis;

  for (i=0; i<MechsimOptions.Data.mass_count; i++)
  {
    for (j=0; j<i; j++)
    /*
       This double loop accesses each combination of
       two different masses exactly once
    */
    {
      VSub(Axis, Pos_Array[i], Pos_Array[j] );
      VLength(lAxis, Axis);

			if (lAxis>0)  // otherwise we can't conclude the direction
			{
				VInverseScaleEq(Axis, lAxis); // Normalize direction vector

				for (k=0; k<MechsimOptions.Interactions.count; k++)
				{
					FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Interactions.Function[k]));

					// evaluate function for first mass
					POVFPU_SetLocal(X, Pos_Array[i][X]);
					POVFPU_SetLocal(Y, Pos_Array[i][Y]);
					POVFPU_SetLocal(Z, Pos_Array[i][Z]);
					if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
					// additional mass 1 parameter
					if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[i].Mass);
					// additional mass 2 parameter
					if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[j].Mass);

					Fn_Val1 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));

					// evaluate function for second mass
					POVFPU_SetLocal(X, Pos_Array[j][X]);
					POVFPU_SetLocal(Y, Pos_Array[j][Y]);
					POVFPU_SetLocal(Z, Pos_Array[j][Z]);
					if (f->parameter_cnt > 3) POVFPU_SetLocal(T, lAxis); // additional distance parameter
					// additional mass 1 parameter
					if (f->parameter_cnt > 4) POVFPU_SetLocal(T+1, MechsimOptions.Data.Masses[j].Mass);
					// additional mass 2 parameter
					if (f->parameter_cnt > 5) POVFPU_SetLocal(T+2, MechsimOptions.Data.Masses[i].Mass);

					Fn_Val2 = POVFPU_Run(*(MechsimOptions.Interactions.Function[k]));

					if (fabs(Fn_Val1) > MECHSIM_EPSILON)
					{
						/* apply force to first mass */
						VAddScaledEq(Accel[i],  -Fn_Val1/MechsimOptions.Data.Masses[i].Mass, Axis);
					}
					if (fabs(Fn_Val2) > MECHSIM_EPSILON)
					{
						/* apply force to second mass */
						VAddScaledEq(Accel[j], Fn_Val2/MechsimOptions.Data.Masses[j].Mass, Axis);
					}
				}
      }
    }
  }

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradient_Fields
*
* INPUT
*
*   Index - index of the mass
*   Accel - accelerations of the masses
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated acceleration is added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Gradient descent version for the field forces
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Nov 2002 : Added custom force fields
*
******************************************************************************/

void Mechsim_Gradient_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array)
{
  int k, param;

  VAddEq( Accel, MechsimOptions.gravity);

  for (k=0; k<MechsimOptions.Fields.count; k++)
  {
    FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Fields.Function[k]));

    POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
    POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
    POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);

    (void)POVFPU_Run(*(MechsimOptions.Fields.Function[k]));

    if (f->return_size >= 3)
      for(param = 0; param < 3; param++)
        Accel[param] += POVFPU_GetLocal(param);
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Force_Fields
*
* INPUT
*
*   Index - index of the mass
*   Accel - accelerations of the masses
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated acceleration is added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the field forces on a point mass
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Nov 2002 : Added custom force fields
*
******************************************************************************/

void Mechsim_Force_Fields(int Index, VECTOR Accel, VECTOR *Pos_Array)
{
  int k, param;

  VAddEq( Accel, MechsimOptions.gravity);

  for (k=0; k<MechsimOptions.Fields.count; k++)
  {
    FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Fields.Function[k]));

    POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
    POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
    POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);

    (void)POVFPU_Run(*(MechsimOptions.Fields.Function[k]));

    if (f->return_size >= 3)
      for(param = 0; param < 3; param++)
        Accel[param] += POVFPU_GetLocal(param) / MechsimOptions.Data.Masses[Index].Mass;
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradient_Forces
*
* INPUT
*
*   Index - index of the mass
*   Accel - accelerations of the masses
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated acceleration is added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Gradient descent version for the custom forces function
*
* CHANGES
*
*   --- Apr 2005 : Creation
*
******************************************************************************/

void Mechsim_Gradient_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array)
{
  int param;

	if (MechsimOptions.Forces.count>MechsimOptions.Data.Masses[Index].Force)
	{
		FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));

		POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
		POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
		POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);

		(void)POVFPU_Run(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));

		if (f->return_size >= 3)
			for(param = 0; param < 3; param++)
				Accel[param] += POVFPU_GetLocal(param);
	}

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Force_Forces
*
* INPUT
*
*   Index - index of the mass
*   Accel - accelerations of the masses
*   Pos_Array - positions of the masses
*
* OUTPUT
*
*   calculated acceleration is added to 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Calculates the custom forces on a point mass
*
* CHANGES
*
*   --- Apr 2005 : Creation
*
******************************************************************************/

void Mechsim_Force_Forces(int Index, VECTOR Accel, VECTOR *Pos_Array)
{
  int param;

	if (MechsimOptions.Forces.count>MechsimOptions.Data.Masses[Index].Force)
	{
		FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));

		POVFPU_SetLocal(X + f->return_size, Pos_Array[Index][X]);
		POVFPU_SetLocal(Y + f->return_size, Pos_Array[Index][Y]);
		POVFPU_SetLocal(Z + f->return_size, Pos_Array[Index][Z]);

		(void)POVFPU_Run(*(MechsimOptions.Forces.Function[MechsimOptions.Data.Masses[Index].Force]));

		if (f->return_size >= 3)
			for(param = 0; param < 3; param++)
				Accel[param] += POVFPU_GetLocal(param) / MechsimOptions.Data.Masses[Index].Mass;

	}

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Gradients
*
* INPUT
*
*   Pos_Array - state of the masses
*   Vel_Array - *** Different usage here ***
*               stores the last position of the mass
*   Time - current time index for the environment function
*
* OUTPUT
*
*   calculated acceleration is returned via 'Accel'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Wrapper for calculating the gradients of the potential fields
*
* CHANGES
*
*   --- Sep 2002 : Creation
*       Feb 2005 : added support for global fixed
*       Apr 2005 : added custom Forces
*
******************************************************************************/

void Mechsim_Gradients(VECTOR *Gradient, VECTOR *Pos_Array, VECTOR *Vel_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  int i, j;
	bool Fixed;

	if (MechsimOptions.Fixed[0] || MechsimOptions.Fixed[1] || MechsimOptions.Fixed[2])
		Fixed = true;
	else
		Fixed = false;

  /* first set all Gradients to 0.0 */
  for (i=0; i<MechsimOptions.Data.mass_count; i++)
    Make_Vector(Gradient[i], 0.0, 0.0, 0.0);

  /* calculate gradients that apply to several masses */
	if ((MechsimOptions.Calc_Collision != MECHSIM_COLLISION_NONE) ||
			(MechsimOptions.Calc_Collision_Connections  != MECHSIM_COLLISION_NONE) ||
			(MechsimOptions.Calc_Collision_Faces  != MECHSIM_COLLISION_NONE))
    Mechsim_Collide_Dynamic(Gradient, Pos_Array, NULL, Bounding_Data);

  if (MechsimOptions.Interactions.count>0)
    Mechsim_Gradient_Interactions(Gradient, Pos_Array);

  if (MechsimOptions.Data.connect_count>0)
    Mechsim_Gradient_Connect(Gradient, Pos_Array);

	if (MechsimOptions.Data.ve_count>0)
		Mechsim_Gradient_Viscoelastics(Gradient, Pos_Array);

  for (i=0; i<MechsimOptions.Data.mass_count; i++)
  {
    if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
    {
      /* calculate gradients that only apply to this mass */

			for (j=0; j<MechsimOptions.Env_count; j++) // Environments
      {
				if (MechsimOptions.Environment[j].Function != NULL || MechsimOptions.Environment[j].Object != NULL)
					Mechsim_Gradient_Collide_Environment(i, Gradient[i], Pos_Array, Vel_Array, Time, 
																							 &MechsimOptions.Environment[j]);
      }

			Mechsim_Gradient_Fields(i, Gradient[i], Pos_Array);

			if (MechsimOptions.Forces.count>0)
				Mechsim_Gradient_Forces(i, Gradient[i], Pos_Array);

			/* handle global fixed */
			if (Fixed)
			{
				if (MechsimOptions.Fixed[0]) 
				{
					Gradient[i][X] = 0.0;
				}
				if (MechsimOptions.Fixed[1]) 
				{
					Gradient[i][Y] = 0.0;
				}
				if (MechsimOptions.Fixed[2]) 
				{
					Gradient[i][Z] = 0.0;
				}
			}

    }
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Forces
*
* INPUT
*
*   State_Array - state of the masses (position and velocity)
*   Time - current time index (for the environment function)
*   Bounding_Data - bounding information for collisions
*
* OUTPUT
*
*   calculated Force Vector is returned via 'Forces'.
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Wrapper for calculating all forces on all masses
*
*   ------------------------------------------------------------------
*
*   The arrays are used as follows:
*
*    State_Array - describes the current state of the system
*                  (at time 'Time'):
*
*
*    0           mass_count        mass_count*2
*     -------------------------------
*    |               |               |
*    |   Positions   |  Velocities   |
*    |               |               |
*     -------------------------------
*
*    Forces - contains the 'right side' of the ODE,
*             converted to first order the following way:
*
*                   x'' = f(x, x', t)
*
*              -->  X'  = F(X, t)
*
*             with X = <x, x'> and X' = <x', x''>
*
*              ==>  F(X, t) = <x', f(x, x', t)> 
*
*
*
*    0           mass_count        mass_count*2
*     -------------------------------
*    |               |               |
*    |  Velocities   | Accelerations |
*    |               |               |
*     -------------------------------
*
*   ------------------------------------------------------------------
*
*   The local identifiers 'Vel_Array' and 'Accel' are used to directly 
*   access the second part of these arrays.
*
*
*
*
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Jan 2005 : Modified for new data structures
*       Feb 2005 : added support for global fixed
*       Apr 2005 : added custom Forces
*
******************************************************************************/

void Mechsim_Forces(VECTOR *Forces, VECTOR *State_Array, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	int i, j;
	bool Fixed;
	VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
	VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];

	if (MechsimOptions.Fixed[0] || MechsimOptions.Fixed[1] || MechsimOptions.Fixed[2])
		Fixed = true;
	else
		Fixed = false;

	/* first set all Accelerations to 0.0 and copy Velocities to Forces Array */
	for (i=0; i<MechsimOptions.Data.mass_count; i++)
	{
		Assign_Vector(Forces[i], Vel_Array[i]);
		Make_Vector(Accel[i], 0.0, 0.0, 0.0);
	}

	/* calculate forces that apply to several masses */
	if ((MechsimOptions.Calc_Collision != MECHSIM_COLLISION_NONE) ||
			(MechsimOptions.Calc_Collision_Connections  != MECHSIM_COLLISION_NONE) ||
			(MechsimOptions.Calc_Collision_Faces  != MECHSIM_COLLISION_NONE))
		Mechsim_Collide_Dynamic(Accel, State_Array, Vel_Array, Bounding_Data);

	if (MechsimOptions.Interactions.count>0)
		Mechsim_Force_Interactions(Accel, State_Array);

	if (MechsimOptions.Data.connect_count>0)
		Mechsim_Force_Connect(Accel, State_Array, Vel_Array);

	if (MechsimOptions.Data.ve_count>0)
		Mechsim_Force_Viscoelastics(Accel, State_Array);

	for (i=0; i<MechsimOptions.Data.mass_count; i++)
	{
		if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* calculate forces that only apply to this mass */
			for (j=0; j<MechsimOptions.Env_count; j++) // Environments
      {
				if (MechsimOptions.Environment[j].Method==2)
        {
					if (MechsimOptions.Environment[j].Function != NULL || MechsimOptions.Environment[j].Object != NULL)
						Mechsim_Impact_Collide_Environment(i, State_Array, Vel_Array, Time,
																							 &MechsimOptions.Environment[j]);
				}
				else
        {
					if (MechsimOptions.Environment[j].Function != NULL || MechsimOptions.Environment[j].Object != NULL)
						Mechsim_Force_Collide_Environment(i, Accel[i], State_Array, Vel_Array, Time,
																							&MechsimOptions.Environment[j]);
				}
      }

			Mechsim_Force_Fields(i, Accel[i], State_Array);

			if (MechsimOptions.Data.Masses[i].Force>=0)
				Mechsim_Force_Forces(i, Accel[i], State_Array);

			/* handle global fixed */
			if (Fixed)
			{
				if (MechsimOptions.Fixed[0]) 
				{
					Accel[i][X] = 0.0;
					Vel_Array[i][X] = 0.0;
				}
				if (MechsimOptions.Fixed[1]) 
				{
					Accel[i][Y] = 0.0;
					Vel_Array[i][Y] = 0.0;
				}
				if (MechsimOptions.Fixed[2]) 
				{
					Accel[i][Z] = 0.0;
					Vel_Array[i][Z] = 0.0;
				}
			}
#if(MECHSIM_DEBUG == 1)
			//				if ((Steps==0) && (i==0)) Debug_Info("\nAcc=<%g, %g, %g>\n", Accel[0][X],Accel[0][Y],Accel[0][Z] );
#endif
    }
		else
		{
			Make_Vector(Accel[i], 0.0, 0.0, 0.0);
			Make_Vector(Vel_Array[i], 0.0, 0.0, 0.0);
		}
  }

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Check_Col
*
* INPUT
*
*   Pos - new position
*   Old_Pos - old position to restore when mass is inside environment shapes
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Penetration prevention for object intersection based environments
*
* CHANGES
*
*   --- Oct 2002 : Creation
*
******************************************************************************/

void Mechsim_Check_Col(VECTOR Pos, VECTOR Old_Pos)
{
	MECHSIM_ENVIRONMENT *Env;

	if (MechsimOptions.Environment != NULL) // Environments
	{
		Env = MechsimOptions.Environment;

		while (Env != NULL)
		{
			if (Env->Function == NULL && Env->Object != NULL) // only for object based environments
				if (Inside_Object(Pos, Env->Object))
				{
					Assign_Vector(Pos, Old_Pos);
					Increase_Counter(stats[MechSim_Environment_Penetrations]);
				}

			Env = Env->Next;
		}
	}

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Check_Instability
*
* INPUT
*
*   Pos - position of mass
*   Vel - velocity of mass
*   Accel - acceleration of mass
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Tries to do a rough estimation if the simulation grows instable and
*   drifts into unrealistic stages. Extreme instabilities might cause
*   program crashes if not detected.
*
* CHANGES
*
*   --- Dec 2002 : Creation
*       Mar 2005 : increased threshold
*
******************************************************************************/

void Mechsim_Check_Instability(VECTOR Pos, VECTOR Vel, VECTOR Accel)
{
	DBL lAcc;

	VLength(lAcc, Accel);

	/*
		simply test if the acceleration exceeds a certain value.
		the exact value is probably not critical since an acceleration
		just a bit lower will be likely to cause an even higher value
		some steps later.
	*/

	if (lAcc > 1.0e200)
		//if (lAcc > 1.0e12)
		Error("Instability detected during simulation.  Using smaller\ntime steps or changing the system parameters might help.\n");

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Euler_Step
*
* INPUT
*
*   Forces - To store the evaluations of the right side of the equation
*   State_Array - new system state (does not need to be initialized)
*   State_Old - old system state
*   Time - current time value
*   Bounding_Data - bounding information for collisions
*
* OUTPUT
*
* RETURNS
*
*   new time = old time + fixed time step
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Runs a single step for numerical integration with Euler method
*
*     ===============================================
*     Euler-Method:
*     ===============================================
*
*     Formula:
*
*       y_i+1 = y_i + h*f(y_i, t)
*
*     Note: y_i/y_i+1 represents both positions and velocities
*
*     For all time steps the forces are calculated,
*     results ('f(y_i, t)') are stored in 'Forces' and then
*     the are applied to all mass positions.
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Extracted function for single time step
*
******************************************************************************/

DBL Mechsim_Euler_Step(VECTOR *Forces, VECTOR *State_Array, VECTOR *State_Old, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	int i, j;

	/* calculate all forces at y_i */
	Mechsim_Forces(Forces, State_Old, Time, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count; i++)
	{
		j = i + MechsimOptions.Data.mass_count;

		if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* y_new = y_old + h*f(y_old) */
			State_Array[i][X] = State_Old[i][X] + MechsimOptions.Time_Step*Forces[i][X];
			State_Array[i][Y] = State_Old[i][Y] + MechsimOptions.Time_Step*Forces[i][Y];
			State_Array[i][Z] = State_Old[i][Z] + MechsimOptions.Time_Step*Forces[i][Z];

			State_Array[j][X] = State_Old[j][X] + MechsimOptions.Time_Step*Forces[j][X];
			State_Array[j][Y] = State_Old[j][Y] + MechsimOptions.Time_Step*Forces[j][Y];
			State_Array[j][Z] = State_Old[j][Z] + MechsimOptions.Time_Step*Forces[j][Z];

			Mechsim_Check_Instability(State_Array[i], State_Array[j], Forces[j]);
		}
		else 
		{
			Assign_Vector(State_Array[i], State_Old[i]);
			Make_Vector(State_Array[j], 0.0, 0.0, 0.0);
		}
		Do_Cooperate(1);
	}

	return Time+MechsimOptions.Time_Step;
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Heun_Step
*
* INPUT
*
*   Forces - To store the evaluations of the right side of the equation
*   State_Array - new system state (does not need to be initialized)
*   State_Old - old system state
*   k1 - in between evaluation
*   Time - current time value
*   Bounding_Data - bounding information for collisions
*
* OUTPUT
*
* RETURNS
*
*   new time = old time + fixed time step
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Runs a single step for numerical integration with Heun method
*
*     ===============================================
*     Heun-Method:
*     ===============================================
*
*     Formula:
*
*       y_i+1 = y_i + (h/2)*f(k_1 + k_2)
*
*     with:
*
*       k_1 = f(y_i, t)
*       k_2 = f(y_i + h*k_1, t + h)
*
*     Note: y_i/y_i+1 represents both positions and velocities
*
*     k_1 is required to be stored for all masses while
*     k_2 is calculated in the loop for 'y_i+1'.  The in between
*     state (y2 = y_i + h*k_1) is stored in Pos2/Vel2.
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Extracted function for single time step
*
******************************************************************************/

DBL Mechsim_Heun_Step(VECTOR *Forces, VECTOR *State_Array, VECTOR *State_Old, VECTOR *k1, DBL Time, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	int i, j;

	/* calculate all forces at y_i */
	Mechsim_Forces(Forces, State_Old, Time, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
	{
		j = i % MechsimOptions.Data.mass_count;

		if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* k1 = f(y_old) */
			Assign_Vector(k1[i], Forces[i]);

			/* y2 = y_old + h*f(y_old) */
			State_Array[i][X] = State_Old[i][X] + MechsimOptions.Time_Step*Forces[i][X];
			State_Array[i][Y] = State_Old[i][Y] + MechsimOptions.Time_Step*Forces[i][Y];
			State_Array[i][Z] = State_Old[i][Z] + MechsimOptions.Time_Step*Forces[i][Z];

			Do_Cooperate(1);
		}
	}

	/* =================================================================== */

	/* calculate all forces at y2, time = t+h */
	Mechsim_Forces(Forces, State_Old, Time+MechsimOptions.Time_Step, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count; i++)
	{
		j = i + MechsimOptions.Data.mass_count;

		if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* y_new = y_old + (h/2)*(k1 + k2) */
			/* k2 = f(y2) */
			State_Array[i][X] = State_Old[i][X] + MechsimOptions.Time_Step*0.5*(k1[i][X]+Forces[i][X]);
			State_Array[i][Y] = State_Old[i][Y] + MechsimOptions.Time_Step*0.5*(k1[i][Y]+Forces[i][Y]);
			State_Array[i][Z] = State_Old[i][Z] + MechsimOptions.Time_Step*0.5*(k1[i][Z]+Forces[i][Z]);

			State_Array[j][X] = State_Old[j][X] + MechsimOptions.Time_Step*0.5*(k1[j][X]+Forces[j][X]);
			State_Array[j][Y] = State_Old[j][Y] + MechsimOptions.Time_Step*0.5*(k1[j][Y]+Forces[j][Y]);
			State_Array[j][Z] = State_Old[j][Z] + MechsimOptions.Time_Step*0.5*(k1[j][Z]+Forces[j][Z]);

			Mechsim_Check_Instability(State_Array[i], State_Array[j], Forces[j]);
		}
		else 
		{
			Assign_Vector(State_Array[i], State_Old[i]);
			Make_Vector(State_Array[j], 0.0, 0.0, 0.0);
		}
		Do_Cooperate(1);
	}

	/* =================================================================== */

	return Time+MechsimOptions.Time_Step;
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Runge_Kutta4_Step
*
* INPUT
*
*   Forces - To store the evaluations of the right side of the equation
*   State_Array - new system state (does not need to be initialized)
*   State_Old - old system state
*   k1,k2,k3 - in between evaluations
*   Do_k1 - if false k1 is not evaluated (because it has been before)
*   Time - current time value
*   Time_Step - current time step
*   Bounding_Data - bounding information for collisions
*
* OUTPUT
*
* RETURNS
*
*   new time = old time + time step
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Runs a single step for numerical integration with 4th order Runge-Kutta method
*
*     ===============================================
*     4th order Runge-Kutta method:
*     ===============================================
*
*     Formula:
*
*       y_i+1 = y_i + (h/6)*f(k_1 + 2*k_2 + 2*k_3 + k_4)
*
*     with:
*
*       k_1 = f(y_i, t)
*       k_2 = f(y_i + (h/2)*k_1, t + (h/2))
*       k_3 = f(y_i + (h/2)*k_2, t + (h/2))
*       k_4 = f(y_i + h*k_3, t + h)
*
*     Note: y_i/y_i+1 represents both positions and velocities
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Extracted function for single time step
*
******************************************************************************/

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)
{
	int i, j;

	/* calculate all forces at y_i */
	Mechsim_Forces(Forces, State_Old, Time, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
	{
		j = i % MechsimOptions.Data.mass_count;
	
		if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* k1 = f(y_old) */
			Assign_Vector(k1[i], Forces[i]);

			/* y2 = y_old + (h/2)*k1 */
			State_Array[i][X] = State_Old[i][X] + Time_Step*0.5*Forces[i][X];
			State_Array[i][Y] = State_Old[i][Y] + Time_Step*0.5*Forces[i][Y];
			State_Array[i][Z] = State_Old[i][Z] + Time_Step*0.5*Forces[i][Z];

			Do_Cooperate(1);
		}
	}

	/* =================================================================== */

	/* calculate all forces at y2, time = t + (h/2) */
	Mechsim_Forces(Forces, State_Array, Time + Time_Step*0.5, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
	{
		j = i % MechsimOptions.Data.mass_count;

		if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* k2 = f(y_old + (h/2)*k1, t + (h/2)) */
			Assign_Vector(k2[i], Forces[i]);

			/* y3 = y_old + (h/2)*k_2 */
			State_Array[i][X] = State_Old[i][X] + Time_Step*0.5*Forces[i][X];
			State_Array[i][Y] = State_Old[i][Y] + Time_Step*0.5*Forces[i][Y];
			State_Array[i][Z] = State_Old[i][Z] + Time_Step*0.5*Forces[i][Z];

			Do_Cooperate(1);
		}
	}

	/* =================================================================== */

	/* calculate all forces at y3, time = t + (h/2) */
	Mechsim_Forces(Forces, State_Array, Time + Time_Step*0.5, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count*2; i++)
	{
		j = i % MechsimOptions.Data.mass_count;

		if (!( MechsimOptions.Data.Masses[j].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* k3 = f(y_old + (h/2)*k_2, t + (h/2)) */
			Assign_Vector(k3[i], Forces[i]);

			/* y4 = y_old + h*k_3 */
			State_Array[i][X] = State_Old[i][X] + Time_Step*Forces[i][X];
			State_Array[i][Y] = State_Old[i][Y] + Time_Step*Forces[i][Y];
			State_Array[i][Z] = State_Old[i][Z] + Time_Step*Forces[i][Z];

			Do_Cooperate(1);
		}
	}

	/* =================================================================== */

	/* calculate all forces at y4, time = t + h */
	Mechsim_Forces(Forces, State_Array, Time + Time_Step, Bounding_Data);
	if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

	for (i=0; i<MechsimOptions.Data.mass_count; i++)
	{
		j = i + MechsimOptions.Data.mass_count;

		if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
		{
			/* y_new = y_old + (h/6)*(k1 + 2*k2 + 2*k3 + k4) */
			/* k4 = f(y_old + h*k_3, t + h) */
			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]);
			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]);
			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]);

			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]);
			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]);
			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]);

			Mechsim_Check_Instability(State_Array[i], State_Array[j], Forces[j]);
		}
		else 
		{
			Assign_Vector(State_Array[i], State_Old[i]);
			Make_Vector(State_Array[j], 0.0, 0.0, 0.0);
		}
		Do_Cooperate(1);
	}
	/* =================================================================== */

	return (Time+Time_Step);
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Runge_Kutta4_Step_Adaptive
*
* INPUT
*
*   Forces - To store the evaluations of the right side of the equation
*   State_Array - new system state (does not need to be initialized)
*   State_Old - old system state
*   State2, State3 - large and in between step
*   k1,k2,k3 - in between evaluations
*   Time - current time value
*   Bounding_Data - bounding information for collisions
*
* OUTPUT
*
* RETURNS
*
*   new time = old time + current time step
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Runs a 4th order Runge-Kutta step and adjusts the time stepping
*
*   Idea based on example in 'Numerical Recipes in C'
*
* CHANGES
*
*   --- Feb 2005 : Creation
*
******************************************************************************/

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)
{
	DBL Time_New;
	DBL Time_Step;
	DBL Err;
	DBL Error_max;
	int i;
	/*

	Time_New = Mechsim_Runge_Kutta4_Step(Forces, State_Array, State_Old, k1, k2, k3, true, Time, MechsimOptions.Time_Step, Bounding_Data);

#if(MECHSIM_DEBUG == 1)
	Debug_Info("Runge_Kutta4 Non-Adaptive %g\n", MechsimOptions.Time_Step);
#endif

	return (Time_New);
	*/

	Time_Step = MechsimOptions.Time_Step;

	while (true)
	{
		/* do integration step (large) */
		Time_New = Mechsim_Runge_Kutta4_Step(Forces, State2, State_Old, k1, k2, k3, true, Time, Time_Step*2.0, Bounding_Data);
		/* do integration step (2x small) */
		Time_New = Mechsim_Runge_Kutta4_Step(Forces, State3, State_Old, k1, k2, k3, false, Time, Time_Step, Bounding_Data);
		Time_New = Mechsim_Runge_Kutta4_Step(Forces, State_Array, State3, k1, k2, k3, true, Time_New, Time_Step, Bounding_Data);

		Error_max = 0.0;
		for (i=0; i<MechsimOptions.Data.mass_count; i++)
		{
			VDist(Err, State2[i], State_Array[i]);
			Error_max = max(Error_max, Err);
		}

		Error_max /= MechsimOptions.Accuracy;

		/* time step small enough */
		if ((Error_max <= 1.0) && (Error_max >= 0.0) )	break;

		/* otherwise reduce step size */
		Time_Step *= max(0.1, Mechsim_Safety_Fact*pow(Error_max, -Mechsim_Exp_Dec));
		if (Time_Step < MechsimOptions.Time_Step_Min)
			Error("Adaptive time stepping suggested too small time steps (%g).\n", Time_Step);

	}

	if (Error_max > Mechsim_Error_Threshold) Time_Step *= Mechsim_Safety_Fact*pow(Error_max, -Mechsim_Exp_Inc); 
	else Time_Step *= 5.0;

	if ((Time_Step * 5.0) > (MechsimOptions.End_Time-MechsimOptions.Start_Time))
		Time_Step = (MechsimOptions.End_Time-MechsimOptions.Start_Time)/5.0001;

	MechsimOptions.Time_Step = Time_Step;

	// for stats
	MechsimOptions.TS_Min = min(MechsimOptions.TS_Min, Time_Step); 
	MechsimOptions.TS_Max = max(MechsimOptions.TS_Max, Time_Step); 

	return (Time_New);
}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Integrate_Euler
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Do numerical integration with Euler method.
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Modified for new data structures and moved integration step
*                  into separate function
*       Apr 2005 : moved out Forces allocation
*
******************************************************************************/

void Mechsim_Integrate_Euler(VECTOR *State_Array, VECTOR *Forces)
{
	int i, j;

	VECTOR *State_Old;
	VECTOR *Pos_Array = State_Array;
	VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
	VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];

	VECTOR *Attach_Move = NULL;
	VECTOR *Attach_Move_Vel = NULL;

	int param;

	MECHSIM_BOUNDING_DATA* Bounding_Data;

	Bounding_Data = init_bounding();
  
	if (MechsimOptions.Attachments.count>0)
	{
		/* Attachments movement data cache */
		Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
		Attach_Move_Vel = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
	}
  
	/* Allocate additional data blocks for integration */
	State_Old = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");

	MechsimOptions.Time = MechsimOptions.Start_Time;
	MechsimOptions.Steps=0;

	while (MechsimOptions.Time < MechsimOptions.End_Time)
	{
		Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
		Do_Cooperate(0);

		POV_MEMCPY(State_Old, State_Array, MechsimOptions.Data.mass_count*2*sizeof (VECTOR));

		/* do integration step */
		MechsimOptions.Time = Mechsim_Euler_Step(Forces, State_Array, State_Old, MechsimOptions.Time, Bounding_Data);
		MechsimOptions.Steps++;

		/* calculate the movement of the attachments during this time step */
		if (MechsimOptions.Attachments.count>0)
		{
			for (i=0; i<MechsimOptions.Attachments.count; i++)
			{
				FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));

				POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
				(void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
				for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);

				POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
				(void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
				for(param = 0; param < min((int)f->return_size, 3); param++)
				{
					Attach_Move[i][param] -= POVFPU_GetLocal(param);
					Attach_Move_Vel[i][param] = Attach_Move[i][param]/MechsimOptions.Time_Step;
				}
			}
		}

		/* apply attachments and penetration prevention for environments */
		for (i=0; i<MechsimOptions.Data.mass_count; i++)
		{
			if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
			{
				j = i + MechsimOptions.Data.mass_count;

				Do_Cooperate(1);

				if (MechsimOptions.Data.Masses[i].Attach >= 0)
				{
					/* move the attached masses */
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][0])
					{
						State_Array[i][X] = State_Old[i][X] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][X];
						State_Array[j][X] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][X];
					}
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][1])
					{
						State_Array[i][Y] = State_Old[i][Y] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Y];
						State_Array[j][Y] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Y];
					}
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][2])
					{
						State_Array[i][Z] = State_Old[i][Z] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Z];
						State_Array[j][Z] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Z];
					}
				}

				Mechsim_Check_Col(State_Array[i], State_Old[i]);
			}
		}

		/* integrate the inner variables of viscoelastic elements */
		if (MechsimOptions.Data.ve_count>0) Mechsim_Integrate_Viscoelastics(State_Array);

		Increase_Counter(stats[MechSim_Steps]);

	}

	POV_FREE(State_Old);

	if (MechsimOptions.Attachments.count>0)
	{
		POV_FREE(Attach_Move);
		POV_FREE(Attach_Move_Vel);
	}

	free_bounding(Bounding_Data);

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Integrate_Heun
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Do numerical integration with Heun method.
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Modified for new data structures
*       Apr 2005 : moved out Forces allocation
*
******************************************************************************/

void Mechsim_Integrate_Heun(VECTOR *State_Array, VECTOR *Forces)
{
	int i, j;

	VECTOR *State_Old;
	VECTOR *k1;

	VECTOR *Pos_Array = State_Array;
	VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
	VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];

	VECTOR *Attach_Move = NULL;
	VECTOR *Attach_Move_Vel = NULL;

	int param;

	MECHSIM_BOUNDING_DATA* Bounding_Data;

	Bounding_Data = init_bounding();

	/* Attachments movement data cache  */
	if (MechsimOptions.Attachments.count>0)
	{
		Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
		Attach_Move_Vel = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
	}

	/* Allocate additional data blocks for integration */
	State_Old = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
	k1 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");

	while (MechsimOptions.Time < MechsimOptions.End_Time)
	{
		Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
		Do_Cooperate(0);

		POV_MEMCPY(State_Old, State_Array, MechsimOptions.Data.mass_count*2*sizeof (VECTOR));

		/* do integration step */
		MechsimOptions.Time = Mechsim_Heun_Step(Forces, State_Array, State_Old, k1, MechsimOptions.Time, Bounding_Data);
		MechsimOptions.Steps++;

		/* calculate the movement of the attachments during this time step */
		if (MechsimOptions.Attachments.count>0)
		{
			for (i=0; i<MechsimOptions.Attachments.count; i++)
			{
				FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));

				POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
				(void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
				for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);

				POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
				(void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
				for(param = 0; param < min((int)f->return_size, 3); param++)
				{
					Attach_Move[i][param] -= POVFPU_GetLocal(param);
					Attach_Move_Vel[i][param] = Attach_Move[i][param]/MechsimOptions.Time_Step;
				}
			}
		}

		/* apply attachments and penetration prevention for environments */
		for (i=0; i<MechsimOptions.Data.mass_count; i++)
		{
			if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
			{
				j = i + MechsimOptions.Data.mass_count;

				Do_Cooperate(1);

				if (MechsimOptions.Data.Masses[i].Attach >= 0)
				{
					/* move the attached masses */
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][0])
					{
						State_Array[i][X] = State_Old[i][X] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][X];
						State_Array[j][X] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][X];
					}
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][1])
					{
						State_Array[i][Y] = State_Old[i][Y] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Y];
						State_Array[j][Y] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Y];
					}
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][2])
					{
						State_Array[i][Z] = State_Old[i][Z] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Z];
						State_Array[j][Z] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Z];
					}
				}

				Mechsim_Check_Col(State_Array[i], State_Old[i]);
			}
		}
		/* =================================================================== */

		// integrate the inner variables of viscoelastic elements
		if (MechsimOptions.Data.ve_count>0) Mechsim_Integrate_Viscoelastics(State_Array);

		Increase_Counter(stats[MechSim_Steps]);

	}

	POV_FREE(k1);
	POV_FREE(State_Old);

	if (MechsimOptions.Attachments.count>0)
	{
		POV_FREE(Attach_Move);
		POV_FREE(Attach_Move_Vel);
	}

	free_bounding(Bounding_Data);

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Integrate_Runge_Kutta4
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Do numerical integration with 4th order Runge Kutta method.
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Modified for new data structures
*       Apr 2005 : moved out Forces allocation
*
******************************************************************************/

void Mechsim_Integrate_Runge_Kutta4(VECTOR *State_Array, VECTOR *Forces)
{
	int i, j;

	VECTOR *State_Old;
	VECTOR *State2;
	VECTOR *State3;
	VECTOR *k1;
	VECTOR *k2;
	VECTOR *k3;

	VECTOR *Pos_Array = State_Array;
	VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
	VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];

	VECTOR *Attach_Move = NULL;
	VECTOR *Attach_Move_Vel = NULL;

	int param;

	MECHSIM_BOUNDING_DATA* Bounding_Data;

	Bounding_Data = init_bounding();

	/* Attachments movement data cache  */
	if (MechsimOptions.Attachments.count>0)
	{
		Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
		Attach_Move_Vel = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");
	}

	/* Allocate additional data blocks for integration */
	State_Old = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
	State2 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
	State3 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
	k1 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
	k2 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
	k3 = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");

	while (MechsimOptions.Time < MechsimOptions.End_Time)
	{
		Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
		Do_Cooperate(0);

		POV_MEMCPY(State_Old, State_Array, MechsimOptions.Data.mass_count*2*sizeof (VECTOR));

		/* do integration step (adaptive) */
		//Time = Mechsim_Runge_Kutta4_Step(Forces, State_Array, State_Old, k1, k2, k3, true, MechsimOptions.Time, MechsimOptions.Time_Step, Bounding_Data);
		MechsimOptions.Time = Mechsim_Runge_Kutta4_Step_Adaptive(Forces, State_Array, State_Old, State2, State3, k1, k2, k3, MechsimOptions.Time, Bounding_Data);
		MechsimOptions.Steps++;

		/* calculate the movement of the attachments during this time step */
		if (MechsimOptions.Attachments.count>0)
		{
			for (i=0; i<MechsimOptions.Attachments.count; i++)
			{
				FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));

				POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
				(void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
				for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);

				POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
				(void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
				for(param = 0; param < min((int)f->return_size, 3); param++)
				{
					Attach_Move[i][param] -= POVFPU_GetLocal(param);
					Attach_Move_Vel[i][param] = Attach_Move[i][param]/MechsimOptions.Time_Step;
				}
			}
		}

		/* apply attachments and penetration prevention for environments */
		for (i=0; i<MechsimOptions.Data.mass_count; i++)
		{
			if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
			{
				if (MechsimOptions.Data.Masses[i].Attach >= 0)
				{
					j = i + MechsimOptions.Data.mass_count;

					Do_Cooperate(1);

					/* move the attached masses */
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][0])
					{
						State_Array[i][X] = State_Old[i][X] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][X];
						State_Array[j][X] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][X];
					}
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][1])
					{
						State_Array[i][Y] = State_Old[i][Y] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Y];
						State_Array[j][Y] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Y];
					}
					if (MechsimOptions.Attachments.Fixed[MechsimOptions.Data.Masses[i].Attach][2])
					{
						State_Array[i][Z] = State_Old[i][Z] + Attach_Move[MechsimOptions.Data.Masses[i].Attach][Z];
						State_Array[j][Z] = Attach_Move_Vel[MechsimOptions.Data.Masses[i].Attach][Z];
					}
				}

				Mechsim_Check_Col(State_Array[i], State_Old[i]);
			}
		}
		/* =================================================================== */

		// integrate the inner variables of viscoelastic elements
		if (MechsimOptions.Data.ve_count>0) Mechsim_Integrate_Viscoelastics(State_Array);

		Increase_Counter(stats[MechSim_Steps]);

	}

	POV_FREE(k1);
	POV_FREE(k2);
	POV_FREE(k3);
	POV_FREE(State_Old);
	POV_FREE(State2);
	POV_FREE(State3);

	if (MechsimOptions.Attachments.count>0)
	{
		POV_FREE(Attach_Move);
		POV_FREE(Attach_Move_Vel);
	}

	free_bounding(Bounding_Data);

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Integrate_Gradient
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Do Gradient descent method
*
*     ===============================================
*     Gradient-descent-Method:
*     ===============================================
*
* CHANGES
*
*   --- Apr 2003 : Moved out of Mechsim_Simulate()
*       Jan 2005 : Modified for new data structures
*       Apr 2005 : moved out Accel allocation
*
******************************************************************************/

void Mechsim_Integrate_Gradient(VECTOR *State_Array, VECTOR *Forces)
{
  int i, j;

	VECTOR *Pos_Array = State_Array;
	VECTOR *Vel_Array = &State_Array[MechsimOptions.Data.mass_count];
  VECTOR *Accel = &Forces[MechsimOptions.Data.mass_count];
  VECTOR *Attach_Move = NULL;

  int param;

  MECHSIM_BOUNDING_DATA* Bounding_Data;
     
  Bounding_Data = init_bounding();

  /* Attachments movement data cache  */
  if (MechsimOptions.Attachments.count>0)
    Attach_Move = (VECTOR *)POV_MALLOC(MechsimOptions.Attachments.count*sizeof (VECTOR), "mechanics simulation data block");

	POV_MEMCPY(Vel_Array, Pos_Array, MechsimOptions.Data.mass_count*sizeof (VECTOR));

	while (MechsimOptions.Time < MechsimOptions.End_Time)
  {
    Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION);
    Do_Cooperate(0);

    /* calculate the movement of the attachments during this time step */
    if (MechsimOptions.Attachments.count>0)
    {
      for (i=0; i<MechsimOptions.Attachments.count; i++)
      {
        FunctionCode *f = POVFPU_GetFunction(*(MechsimOptions.Attachments.Function[i]));

        POVFPU_SetLocal(f->return_size, MechsimOptions.Time+MechsimOptions.Time_Step);
        (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
        for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] = POVFPU_GetLocal(param);

        POVFPU_SetLocal(f->return_size, MechsimOptions.Time);
        (void)POVFPU_Run(*(MechsimOptions.Attachments.Function[i]));
        for(param = 0; param < min((int)f->return_size, 3); param++) Attach_Move[i][param] -= POVFPU_GetLocal(param);
      }
    }

    /* calculate all gradients */
    Mechsim_Gradients(Accel, Pos_Array, Vel_Array, MechsimOptions.Time, Bounding_Data);
	  if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH) Bounding_Data->time_stamp++;

    Do_Cooperate(1);

    for (i=0; i<MechsimOptions.Data.mass_count; i++)
    {
      if (!( MechsimOptions.Data.Masses[i].Flag & MECHSIM_FLAG_FIXED ))
      {

				// store last position to allow object intersection based environments
				// see function 'Mechsim_Gradient_Collide_Environment()' for how this is used
				Assign_Vector(Vel_Array[i], Pos_Array[i]);

				/* move masses in direction of the gradient */
				Pos_Array[i][X] = Pos_Array[i][X] + MechsimOptions.Time_Step*Accel[i][X];
				Pos_Array[i][Y] = Pos_Array[i][Y] + MechsimOptions.Time_Step*Accel[i][Y];
				Pos_Array[i][Z] = Pos_Array[i][Z] + MechsimOptions.Time_Step*Accel[i][Z];

				Do_Cooperate(1);

				if (MechsimOptions.Data.Masses[i].Attach >= 0)
				{
					/* move the attached masses */
					VAdd(Pos_Array[i], Vel_Array[i], Attach_Move[MechsimOptions.Data.Masses[i].Attach]);
				}

				Mechsim_Check_Instability(Pos_Array[i], Accel[i], Accel[i]);
				Mechsim_Check_Col(Pos_Array[i], Vel_Array[i]);
      }
    }

    MechsimOptions.Time += MechsimOptions.Time_Step;
    MechsimOptions.Steps++;

    Increase_Counter(stats[MechSim_Steps]);

  }

  POV_FREE(Accel);

  if (MechsimOptions.Attachments.count>0)
    POV_FREE(Attach_Move);

  // make sure we save zero velocity to file
  for (i=0; i<MechsimOptions.Data.mass_count; i++)
  {
    Vel_Array[i][X] = 0.0;
    Vel_Array[i][Y] = 0.0;
    Vel_Array[i][Z] = 0.0;
  }

  free_bounding(Bounding_Data);

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Simulate
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Do the actual mechanics simulation
*
* CHANGES
*
*   --- Aug 2002 : Creation
*
******************************************************************************/

void Mechsim_Simulate(void)
{
	int i;

	VECTOR *State_Array;
	VECTOR *Forces;

	if ((MechsimOptions.Timing_Given || MechsimOptions.Timing_Loaded) && 
			(MechsimOptions.Start_Time < MechsimOptions.End_Time))
	{

		/* Bounding system by Daniel Jungmann */
		if ((MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_AUTO) &&
				(MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_NO) &&
				(MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_BBOX) &&
			  (MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_HASH))
		{
			Error("mechanics simulation bounding method %d not supported", MechsimOptions.Bounding);
		}

		if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_AUTO)
    {
			/* Just turn bounding on if it would be faster */
			if (
					((MechsimOptions.Data.max_masses < MECHSIM_BOUNDING_HASH_THRESHOLD) || 
					 (MechsimOptions.Data.max_faces < MECHSIM_BOUNDING_HASH_THRESHOLD)) && 
					(MechsimOptions.Data.max_masses*MechsimOptions.Data.max_faces < (MECHSIM_BOUNDING_HASH_THRESHOLD*MECHSIM_BOUNDING_HASH_THRESHOLD))
				 )
	    {
				if (
						((MechsimOptions.Data.max_masses < MECHSIM_BOUNDING_BOX_THRESHOLD) || 
						 (MechsimOptions.Data.max_faces < MECHSIM_BOUNDING_BOX_THRESHOLD)) && 
						(MechsimOptions.Data.max_masses*MechsimOptions.Data.max_faces < (MECHSIM_BOUNDING_BOX_THRESHOLD*MECHSIM_BOUNDING_BOX_THRESHOLD))
					 )
				{
					MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_NO;
				}
				else
			  {
					MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_BBOX;
				}
			}
			else MechsimOptions.Bounding = MECHSIM_FACE_BOUNDING_HASH;
		}

		MechsimOptions.TS_Min = MechsimOptions.TS_Max = MechsimOptions.Time_Step;
		MechsimOptions.Time = MechsimOptions.Start_Time;
		MechsimOptions.Steps=0;

    Send_Progress("Calculating Mechanics Simulation", PROGRESS_MECHANICS_SIMULATION);

    START_TIME

    /* fill the group data structures */
    Mechsim_Prepare_Data();

    /* Allocate data blocks for integration */
    State_Array = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");
		Forces = (VECTOR *)POV_MALLOC(MechsimOptions.Data.mass_count*2*sizeof (VECTOR), "mechanics simulation data block");

    /* Read data from MechsimOptions.Data  */
    for (i=0; i<MechsimOptions.Data.mass_count; i++)
    {
      Assign_Vector(State_Array[i], MechsimOptions.Data.Masses[i].Position);
      Assign_Vector(State_Array[i+MechsimOptions.Data.mass_count], MechsimOptions.Data.Masses[i].Velocity);
    }

#if(MECHSIM_DEBUG == 1)
		Debug_Info("==========================================================\n");
		Debug_Info("Mechsim Options:\n");

		Debug_Info("  MechsimOptions.Method: %d\n", MechsimOptions.Method);
		Debug_Info("  MechsimOptions.Step_Count: %d\n", MechsimOptions.Step_Count);
		Debug_Info("  MechsimOptions.Time_Step: %g\n", MechsimOptions.Time_Step);
		Debug_Info("  MechsimOptions.Start_Time: %g\n", MechsimOptions.Start_Time);
		Debug_Info("  MechsimOptions.End_Time: %g\n", MechsimOptions.End_Time);
		Debug_Info("  MechsimOptions.Timing_Given: %d\n", MechsimOptions.Timing_Given);

	// for adaptive time stepping
		Debug_Info("  MechsimOptions.Accuracy: %g\n", MechsimOptions.Accuracy);
		Debug_Info("  MechsimOptions.Time_Step_Min: %g\n", MechsimOptions.Time_Step_Min);

  // For message output
		Debug_Info("  MechsimOptions.Steps: %d\n", MechsimOptions.Steps);
		Debug_Info("  MechsimOptions.Time: %g\n", MechsimOptions.Time);

		Debug_Info("  MechsimOptions.gravity[Z]: %g\n", MechsimOptions.gravity[Z]);

		Debug_Info("  MechsimOptions.Enabled: %d\n", MechsimOptions.Enabled);

		Debug_Info("  MechsimOptions.Bounding: %d\n", MechsimOptions.Bounding);

		Debug_Info("  MechsimOptions.Data.mass_count: %d\n", MechsimOptions.Data.mass_count);
		Debug_Info("  MechsimOptions.Data.connect_count: %d\n", MechsimOptions.Data.connect_count);
		Debug_Info("  MechsimOptions.Data.faces_count: %d\n", MechsimOptions.Data.faces_count);
		Debug_Info("  MechsimOptions.Data.ve_count: %d\n", MechsimOptions.Data.ve_count);
		Debug_Info("  MechsimOptions.Env_count: %d\n", MechsimOptions.Env_count);
		Debug_Info("  MechsimOptions.Env_max_count: %d\n", MechsimOptions.Env_max_count);

		Debug_Info("==========================================================\n");
#endif


    switch (MechsimOptions.Method)
    {
      case MECHSIM_METHOD_EULER:            // ---- EULER ----
        Mechsim_Integrate_Euler(State_Array, Forces);
        break;

      case MECHSIM_METHOD_HEUN:             // ---- HEUN ----
        Mechsim_Integrate_Heun(State_Array, Forces);
        break;

      case MECHSIM_METHOD_RUNGE_KUTTA4:     // ---- 4th ORDER RUNGE KUTTA ----
        Mechsim_Integrate_Runge_Kutta4(State_Array, Forces);
        break;

      case MECHSIM_METHOD_GRADIENT:
        Mechsim_Integrate_Gradient(State_Array, Forces);
        break;

      default:
        Error("mechanics simulation method %d not supported", MechsimOptions.Method);
        break;
    }

    Send_ProgressUpdate(PROGRESS_MECHANICS_SIMULATION, 0);
    Do_Cooperate(0);

    /* Write data back to MechsimOptions.Data  */
    for (i=0; i<MechsimOptions.Data.mass_count; i++)
    {
      Assign_Vector(MechsimOptions.Data.Masses[i].Position, State_Array[i]);
      Assign_Vector(MechsimOptions.Data.Masses[i].Velocity, State_Array[i+MechsimOptions.Data.mass_count]);
      Assign_Vector(MechsimOptions.Data.Masses[i].Acceleration, Forces[i+MechsimOptions.Data.mass_count]);
    }


    POV_FREE(State_Array);
    POV_FREE(Forces);

    STOP_TIME
    tmechsim = TIME_ELAPSED;
    tmechsim_frame = tmechsim;
    tmechsim_total += tmechsim;

  }

  if (MechsimOptions.Save_File) Mechsim_save_file();

  //back to parsing
  Send_Progress("Parsing", PROGRESS_PARSING);

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Create_Group
*
* INPUT
*
*   Count - number of groups to create
*
* OUTPUT
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Create new group data structures
*
* CHANGES
*
*   --- Sep 2002 : Creation.
*
******************************************************************************/
void Mechsim_Create_Groups(int Count)
{
  int i;

  if (MechsimOptions.Data.max_groups==0) // first time
  {
    MechsimOptions.Data.max_groups = Count;
    MechsimOptions.Data.Groups = (MECHSIM_GROUP *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof (MECHSIM_GROUP), "mechanics simulation group data block");
  }
  else
  {
    if (MechsimOptions.Data.max_groups+Count >= INT_MAX)
    {
      Error("Too many groups in simulation data");
    }
    MechsimOptions.Data.max_groups += Count;
    MechsimOptions.Data.Groups = (MECHSIM_GROUP *)POV_REALLOC(MechsimOptions.Data.Groups, MechsimOptions.Data.max_groups*sizeof (MECHSIM_GROUP), "mechanics simulation group data block");
  }

  // for the start allocate space for 20 masses
  for (i=MechsimOptions.Data.max_groups-Count; i<MechsimOptions.Data.max_groups; i++)
  {
    MechsimOptions.Data.Groups[i].max_masses = 20;
    MechsimOptions.Data.Groups[i].max_connects = 20;
    MechsimOptions.Data.Groups[i].max_faces = 20;

    MechsimOptions.Data.Groups[i].Masses = (MECHSIM_MASS **)POV_MALLOC(MechsimOptions.Data.Groups[i].max_masses*sizeof (MECHSIM_MASS *), "mechanics simulation mass pointer data block");
    MechsimOptions.Data.Groups[i].Connects = (MECHSIM_CONNECT **)POV_MALLOC(MechsimOptions.Data.Groups[i].max_connects*sizeof (MECHSIM_CONNECT *), "mechanics simulation connection pointer data block");
    MechsimOptions.Data.Groups[i].Faces = (MECHSIM_FACE **)POV_MALLOC(MechsimOptions.Data.Groups[i].max_faces*sizeof (MECHSIM_FACE *), "mechanics simulation face pointer data block");

    MechsimOptions.Data.Groups[i].mass_count = 0;
    MechsimOptions.Data.Groups[i].connect_count = 0;
    MechsimOptions.Data.Groups[i].faces_count = 0;
  }

}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_Prepare_Data
*
* INPUT
*
* OUTPUT
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Prepares additional Simulation data structures (groups)
*
*   The groups are arrays of structures like the 'Masses', 'Connects' and
*   'Faces' fields.  The group structure contains arrays of pointers to
*   the actual data.  The purpose of all this is to speed up the collision
*   calculations.
*
*   (Data)
*     |_______(Masses) -> array of MECHSIM_MASS
*     |                                                         pointers to
*     |_______(Connects) -> array of MECHSIM_CONNECT          <------------
*     |                                                                    |
*     |_______(Faces) -> array of MECHSIM_FACE                             |
*     |                                                                    |
*     |_______(Groups) -> array of MECHSIM_GROUP                           |
*                |                                                         |
*                |_______(Masses) -> array of (MECHSIM_MASS *)       |     |
*                |                                                   |     |
*                |_______(Connects) -> array of (MECHSIM_CONNECT *)  |-----
*                |                                                   |
*                |_______(Faces) -> array of (MECHSIM_FACE *)        |
*
*
*
*
* CHANGES
*
*   --- Sep 2002 : Creation.
*
******************************************************************************/
void Mechsim_Prepare_Data(void)
{
  int i, Group;

  /*------ masses ------*/
  for (i=0; i<MechsimOptions.Data.mass_count; i++)
  {
    Group = MechsimOptions.Data.Masses[i].Group;

    // resize group array if necessary
    if ( Group >= MechsimOptions.Data.max_groups )
    {
      Mechsim_Create_Groups(Group-MechsimOptions.Data.max_groups+1);
    }

    if ( Group >= MechsimOptions.Data.group_count ) MechsimOptions.Data.group_count = Group+1;

    // resize mass pointer arrays of the group if necessary
    if ( MechsimOptions.Data.Groups[Group].mass_count >= MechsimOptions.Data.Groups[Group].max_masses )
    {
      if (MechsimOptions.Data.Groups[Group].max_masses >= INT_MAX/2)
      {
        Error("Too many masses in simulation data");
      }
      MechsimOptions.Data.Groups[Group].max_masses *= 2;
      MechsimOptions.Data.Groups[Group].Masses =
        (MECHSIM_MASS **)POV_REALLOC(MechsimOptions.Data.Groups[Group].Masses,
                                     MechsimOptions.Data.Groups[Group].max_masses*
                                     sizeof (MECHSIM_MASS *), "mechanics simulation mass pointer data block");
    }

    // set mass pointer
    MechsimOptions.Data.Groups[Group].Masses[MechsimOptions.Data.Groups[Group].mass_count] = &MechsimOptions.Data.Masses[i];

    // set counter
    MechsimOptions.Data.Groups[Group].mass_count++;

  }

  /*------ connects ------*/
  for (i=0; i<MechsimOptions.Data.connect_count; i++)
  {
    Group = MechsimOptions.Data.Connects[i].Group;

    // resize group array if necessary
    if ( Group >= MechsimOptions.Data.max_groups )
    {
      Mechsim_Create_Groups(Group-MechsimOptions.Data.max_groups+1);
    }

    if ( Group >= MechsimOptions.Data.group_count ) MechsimOptions.Data.group_count = Group+1;

    // resize connect pointer arrays of the group if necessary
    if ( MechsimOptions.Data.Groups[Group].connect_count >= MechsimOptions.Data.Groups[Group].max_connects )
    {
      if (MechsimOptions.Data.Groups[Group].max_connects >= INT_MAX/2)
      {
        Error("Too many connects in simulation data");
      }
      MechsimOptions.Data.Groups[Group].max_connects *= 2;
      MechsimOptions.Data.Groups[Group].Connects =
        (MECHSIM_CONNECT **)POV_REALLOC(MechsimOptions.Data.Groups[Group].Connects,
                                        MechsimOptions.Data.Groups[Group].max_connects*
                                        sizeof (MECHSIM_CONNECT *), "mechanics simulation connect pointer data block");
    }

    // set connect pointer
    MechsimOptions.Data.Groups[Group].Connects[MechsimOptions.Data.Groups[Group].connect_count] = &MechsimOptions.Data.Connects[i];

    // set counter
    MechsimOptions.Data.Groups[Group].connect_count++;

  }

  /*------ faces ------*/
  for (i=0; i<MechsimOptions.Data.faces_count; i++)
  {
    Group = MechsimOptions.Data.Faces[i].Group;

    // resize group array if necessary
    if ( Group >= MechsimOptions.Data.max_groups )
    {
      Mechsim_Create_Groups(Group-MechsimOptions.Data.max_groups+1);
    }

    if ( Group >= MechsimOptions.Data.group_count ) MechsimOptions.Data.group_count = Group+1;

    // resize connect pointer arrays of the group if necessary
    if ( MechsimOptions.Data.Groups[Group].faces_count >= MechsimOptions.Data.Groups[Group].max_faces )
    {
      if (MechsimOptions.Data.Groups[Group].max_faces >= INT_MAX/2)
      {
        Error("Too many faces in simulation data");
      }
      MechsimOptions.Data.Groups[Group].max_faces *= 2;
      MechsimOptions.Data.Groups[Group].Faces =
        (MECHSIM_FACE **)POV_REALLOC(MechsimOptions.Data.Groups[Group].Faces,
                                     MechsimOptions.Data.Groups[Group].max_faces*
                                     sizeof (MECHSIM_FACE *), "mechanics simulation face pointer data block");
    }

    // set connect pointer
    MechsimOptions.Data.Groups[Group].Faces[MechsimOptions.Data.Groups[Group].faces_count] = &MechsimOptions.Data.Faces[i];

    // set counter
    MechsimOptions.Data.Groups[Group].faces_count++;

  }


}


/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_read_file
*
* INPUT
*   file descriptor handle of file (already opened).
*   Group - the group the elements should be sorted into
*           if zero use groups as in file
*
* OUTPUT
*
* RETURNS - Success 1 / failure 0
*
* AUTHOR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Read in a mechanics simulation data file
*
* CHANGES
*
*   --- Aug 2002 : Creation based on radiosity data read function.
*       Feb 2005 : Adding new, more universal file format
*       Apr 2005 : Adding Force index
*
******************************************************************************/
bool Mechsim_read_file(IStream *fd, int Group)
{
	bool got_eof=false;
	long line_num, goodreads;
	bool goodparse, parse_err = false;
	double Param1_DBL, Param2_DBL;
	int Param1_Int;
	int fformat, count, i;
	char file_id[5];
	char line[MAX_STR+2];
	char line_param[MAX_STR+2];

	int mass_count, connect_count, faces_count, ve_count;
	char *Pos;
	VECTOR Posi, Vel;
	double Rad, Mass, Stiff, Damp, Length;
	int Idx1, Idx2, Idx3, Flag, Force, Grp, Attach, ElCnt, Acc;
	int prev_mass_count;

	if ( fd != NULL )
	{
		Send_Progress("Reading Mechsim data from file", PROGRESS_MECHSIM_LOADFILE);

		if ( fd->getline (line, MAX_STR).eof () )
		{
			Warning(0, "Error reading mechanics simulation data (unexpected file end)");
			return false;
		}

		goodreads = 0;
    goodparse = true;

		if (sscanf(line, "%4s, %d,\n", file_id, &fformat) == 2)
			if ((file_id[0] == 'M') &&
					(file_id[1] == 'S') &&
					(file_id[2] == 'I') &&
					(file_id[3] == 'M')
				 )
			{
				// save current mass count to get the connections and faces right.
				// their mass indices have to be increased by this number
				prev_mass_count = MechsimOptions.Data.mass_count;

				line_num = 0;

				switch ( fformat )
				{
					/* --- Old formats for backwards compatibility --- */
					case 1: case 2: case 3:
					{
						if (got_eof = fd->getline (line, MAX_STR).eof ())
							Warning(0, "\nError reading mechanics simulation data (wrong file format)");

						Pos = line;

						if (fformat>2)  // file subtype 3 -> read start time
							Pos = Read_Float(fd, line, Pos, &MechsimOptions.Start_Time, &got_eof);

						Pos = Read_Int(fd, line, Pos, &mass_count, &got_eof);
						Pos = Read_Int(fd, line, Pos, &connect_count, &got_eof);
						Pos = Read_Int(fd, line, Pos, &faces_count, &got_eof);

#if(MECHSIM_DEBUG == 1)
						Debug_Info("header %d %d %d\n", mass_count, connect_count, faces_count);
						//Debug_Info("line %d (%s)\n", line_num, line);
#endif

						while (!got_eof)
						{
							Send_ProgressUpdate(PROGRESS_MECHSIM_LOADFILE);
							Do_Cooperate(0);

							line_num++;

#if(MECHSIM_DEBUG == 1)
							Debug_Info("line %d (%s)\n", line_num, Pos);
							//if (line_num==1) Debug_Info("\n(%s)\n", line);
#endif

							if (line_num <= mass_count)         // ================ masses ===================
							{
								Pos = Read_Vector(fd, line, Pos, Posi, &got_eof);
								Pos = Read_Vector(fd, line, Pos, Vel, &got_eof);
								Pos = Read_Float(fd, line, Pos, &Mass, &got_eof);
								Pos = Read_Float(fd, line, Pos, &Rad, &got_eof);
								Pos = Read_Int(fd, line, Pos, &Flag, &got_eof);

								if (fformat>1) // file contains group specifications
								{
									Pos = Read_Int(fd, line, Pos, &Grp, &got_eof);
									if (Grp<0)
								 	{
										Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
										Grp = 0;
									}
								}
								else Grp = 0;

								if (fformat>2)  // file subtype 3 -> read attachment index
								{
									Pos = Read_Int(fd, line, Pos, &Attach, &got_eof);
								}
								else Attach = -1;

								if ( MechsimOptions.Data.mass_count >= MechsimOptions.Data.max_masses )
								{
									if (MechsimOptions.Data.max_masses >= INT_MAX/2)
									{
										Error("Too many masses in simulation data");
									}
									MechsimOptions.Data.max_masses *= 2;
									MechsimOptions.Data.Masses = (MECHSIM_MASS *)POV_REALLOC(MechsimOptions.Data.Masses, MechsimOptions.Data.max_masses*sizeof (MECHSIM_MASS), "mechanics simulation mass data block");
								}

								Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Position, Posi);
								Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Velocity, Vel);
								MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Mass = Mass;
								MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Radius = Rad;
								MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Flag = Flag;
								MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Attach = Attach;
								MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Force = -1;

								if (Group==0) MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Grp; // no group given -> use those from file
								else MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Group;

								MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Index = MechsimOptions.Data.mass_count;

								MechsimOptions.Data.mass_count++;

#if(MECHSIM_DEBUG == 1)
								Debug_Info("mass load\n");
#endif

								goodreads++;

							}
							else if (line_num <= mass_count+connect_count)    // ================ connections ================
							{
								Pos = Read_Int(fd, line, Pos, &Idx1, &got_eof);
								Pos = Read_Int(fd, line, Pos, &Idx2, &got_eof);
								Pos = Read_Float(fd, line, Pos, &Length, &got_eof);
								Pos = Read_Float(fd, line, Pos, &Stiff, &got_eof);
								Pos = Read_Float(fd, line, Pos, &Damp, &got_eof);

								if (fformat>1) // file contains group specifications
								{
									Pos = Read_Int(fd, line, Pos, &Grp, &got_eof);
									if (Grp<0)
									{
										Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
										Grp = 0;
									}
								}
								else Grp = 0;

								if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count))
								{
									Warning(0, "mechanics simulation data (line %d): connection index out of range, ignoring", line_num);
									continue;
								}

								if ( MechsimOptions.Data.connect_count >= MechsimOptions.Data.max_connects )
								{
									if (MechsimOptions.Data.max_connects >= INT_MAX/2)
									{
										Error("Too many connections in simulation data");
									}
									MechsimOptions.Data.max_connects *= 2;
									MechsimOptions.Data.Connects = (MECHSIM_CONNECT *)POV_REALLOC(MechsimOptions.Data.Connects, MechsimOptions.Data.max_connects*sizeof (MECHSIM_CONNECT), "mechanics simulation connection data block");
								}

								MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx1 = Idx1+prev_mass_count;
								MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx2 = Idx2+prev_mass_count;

								MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Length = Length;
								MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Stiffness = Stiff;
								MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Damping = Damp;
								if (Group==0) MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Grp; // no group given -> use those from file
								else MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Group;

								MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Index = MechsimOptions.Data.connect_count;

								MechsimOptions.Data.connect_count++;

#if(MECHSIM_DEBUG == 1)
								Debug_Info("connect load %d %d %g %g %g\n", Idx1, Idx2, Length, Stiff, Damp);
#endif

								goodreads++;

							}
							else if (line_num <= mass_count+connect_count+faces_count)  // ================ faces ================
							{
								Pos = Read_Int(fd, line, Pos, &Idx1, &got_eof);
								Pos = Read_Int(fd, line, Pos, &Idx2, &got_eof);
								Pos = Read_Int(fd, line, Pos, &Idx3, &got_eof);

								if (fformat>1) // file contains group specifications
								{
									Pos = Read_Int(fd, line, Pos, &Grp, &got_eof);
									if (Grp<0)
									{
										Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
										Grp = 0;
									}
								}
								else Grp = 0;

								if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count) || (Idx3>=MechsimOptions.Data.mass_count))
								{
									Warning(0, "mechanics simulation data (line %d): face index out of range, ignoring", line_num);
									continue;
								}

								if ( MechsimOptions.Data.faces_count >= MechsimOptions.Data.max_faces )
								{
									if (MechsimOptions.Data.max_faces >= INT_MAX/2)
									{
										Error("Too many faces in simulation data");
									}
									MechsimOptions.Data.max_faces *= 2;
									MechsimOptions.Data.Faces = (MECHSIM_FACE *)POV_REALLOC(MechsimOptions.Data.Faces, MechsimOptions.Data.max_faces*sizeof (MECHSIM_FACE), "mechanics simulation face data block");
								}

								MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx1 = Idx1+prev_mass_count;
								MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx2 = Idx2+prev_mass_count;
								MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx3 = Idx3+prev_mass_count;
								if (Group==0) MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Grp; // no group given -> use those from file
								else MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Group;

								MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Index = MechsimOptions.Data.faces_count;

								MechsimOptions.Data.faces_count++;

								goodreads++;

							}
							else got_eof = true;

						}     /* end while-reading loop */

						break;
					}
					/* --- New format - should be upwards compatible --- */
					case 4:
					{
						while (!(got_eof = fd->getline (line, MAX_STR).eof ()) && goodparse)
						{
							Send_ProgressUpdate(PROGRESS_MECHSIM_LOADFILE);
							Do_Cooperate(0);

							line_num++;

							switch ( line[0] )
							{
							  case 'I':    /* Info line with mass, connect and face count (and possibly others) */
								{
									sscanf(line, "I %d %d %d %d", &mass_count, &connect_count, &faces_count, &ve_count);

#if(MECHSIM_DEBUG == 1)
									Debug_Info("Info load: I %d %d %d %d\n", mass_count, connect_count, faces_count, ve_count);
#endif

									break;
								}
							  case 'T':    /* Timing line - Saved start time (and possibly other timing parameters) */
								{
									if (sscanf(line, "T %lf %lf %d", &Param1_DBL, &Param2_DBL, &Param1_Int) >= 3)
									{
										MechsimOptions.End_Time += Param1_DBL - MechsimOptions.Start_Time;
										MechsimOptions.Start_Time = Param1_DBL;
										MechsimOptions.Time = MechsimOptions.Start_Time;
										if (!MechsimOptions.Timing_Given)
										{
											MechsimOptions.Time_Step = Param2_DBL;
											MechsimOptions.Step_Count = Param1_Int;
										}
										/* always load current time step for adaptive method */
										else if (MechsimOptions.Method==3) 
										{
											MechsimOptions.Time_Step = Param2_DBL;
											MechsimOptions.Step_Count = (int)((MechsimOptions.End_Time-MechsimOptions.Start_Time)/MechsimOptions.Time_Step);
										}
										MechsimOptions.Timing_Loaded = true;
									}

#if(MECHSIM_DEBUG == 1)
									Debug_Info("Timing load: I %g %g %d\n", Param1_DBL, Param2_DBL, Param1_Int);
									Debug_Info("  MechsimOptions.Method: %d\n", MechsimOptions.Method);
									Debug_Info("  MechsimOptions.Step_Count: %d\n", MechsimOptions.Step_Count);
									Debug_Info("  MechsimOptions.Time_Step: %g\n", MechsimOptions.Time_Step);
									Debug_Info("  MechsimOptions.Start_Time: %g\n", MechsimOptions.Start_Time);
									Debug_Info("  MechsimOptions.End_Time: %g\n", MechsimOptions.End_Time);
#endif

									break;
								}    
							  case 'M':    /* Mass */
								{
									count  = sscanf(line, "M %lf %lf %lf %lf %lf %lf %lf %lf %d %d %d %d",
																	&Posi[X], &Posi[Y], &Posi[Z],
																	&Vel[X], &Vel[Y], &Vel[Z],
																	&Mass, &Rad, &Flag, &Grp, &Attach, &Force);

									if (count >= 9)
									{
										if ( MechsimOptions.Data.mass_count >= MechsimOptions.Data.max_masses )
										{
											if (MechsimOptions.Data.max_masses >= INT_MAX/2)
											{
												Error("Too many masses in simulation data");
											}
											MechsimOptions.Data.max_masses *= 2;
											MechsimOptions.Data.Masses = (MECHSIM_MASS *)POV_REALLOC(MechsimOptions.Data.Masses, MechsimOptions.Data.max_masses*sizeof (MECHSIM_MASS), "mechanics simulation mass data block");
										}

										if (count < 10) /* no group given */ 
											Grp = 0;
										else if (Grp<0)
										{
											Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
											Grp = 0;
										}

										if (count < 10) /* no attach given */
											Attach = -1;
										if (count < 11) /* no force given */
											Force = -1;

										Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Position, Posi);
										Assign_Vector(MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Velocity, Vel);
										MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Mass = Mass;
										MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Radius = Rad;
										MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Flag = Flag;
										MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Attach = Attach;
										MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Force = Force;

										if (Group==0) // no group given -> use those from file
											MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Grp;
										else MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Group = Group;

										MechsimOptions.Data.Masses[MechsimOptions.Data.mass_count].Index = MechsimOptions.Data.mass_count;

										MechsimOptions.Data.mass_count++;

#if(MECHSIM_DEBUG == 1)
										Debug_Info("mass load\n");
#endif

										goodreads++;
									}
									else
									{
										Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
									}

									break;
								}    
							  case 'C':    /* Connect */
								{
									count  = sscanf(line, "C %d %d %lf %lf %lf %d",
																	&Idx1, &Idx2, &Length, &Stiff, &Damp, &Grp);

									if (count >= 5)
									{
										if (count < 6) /* no group given */ 
											Grp = 0;
										else if (Grp<0)
										{
											Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
											Grp = 0;
										}

										if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count))
										{
											Warning(0, "mechanics simulation data (line %d): connection index out of range, ignoring", line_num);
											continue;
										}

										if ( MechsimOptions.Data.connect_count >= MechsimOptions.Data.max_connects )
										{
											if (MechsimOptions.Data.max_connects >= INT_MAX/2)
											{
												Error("Too many connections in simulation data");
											}
											MechsimOptions.Data.max_connects *= 2;
											MechsimOptions.Data.Connects = (MECHSIM_CONNECT *)POV_REALLOC(MechsimOptions.Data.Connects, MechsimOptions.Data.max_connects*sizeof (MECHSIM_CONNECT), "mechanics simulation connection data block");
										}

										MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx1 = Idx1+prev_mass_count;
										MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Idx2 = Idx2+prev_mass_count;

										MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Length = Length;
										MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Stiffness = Stiff;
										MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Damping = Damp;

										if (Group==0) // no group given -> use those from file
											MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Grp; 
										else MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Group = Group;

										MechsimOptions.Data.Connects[MechsimOptions.Data.connect_count].Index = MechsimOptions.Data.connect_count;

										MechsimOptions.Data.connect_count++;

#if(MECHSIM_DEBUG == 1)
										Debug_Info("connect load %d %d %g %g %g\n", Idx1, Idx2, Length, Stiff, Damp);
#endif

										goodreads++;
									}
									else
									{
										Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
									}

									break;
								}    
							  case 'F':    /* Face */
								{
									count  = sscanf(line, "F %d %d %d %d",	&Idx1, &Idx2, &Idx3, &Grp);

									if (count >= 3)
									{
										if (count < 4) /* no group given */ 
											Grp = 0;
										else if (Grp<0)
										{
											Warning(0, "mechanics simulation data (line %d): group index out of range", line_num);
											Grp = 0;
										}

										if ((Idx1>=MechsimOptions.Data.mass_count) || 
												(Idx2>=MechsimOptions.Data.mass_count) || 
												(Idx3>=MechsimOptions.Data.mass_count))
										{
											Warning(0, "mechanics simulation data (line %d): face index out of range, ignoring", line_num);
											continue;
										}

										if ( MechsimOptions.Data.faces_count >= MechsimOptions.Data.max_faces )
										{
											if (MechsimOptions.Data.max_faces >= INT_MAX/2)
											{
												Error("Too many faces in simulation data");
											}
											MechsimOptions.Data.max_faces *= 2;
											MechsimOptions.Data.Faces = (MECHSIM_FACE *)POV_REALLOC(MechsimOptions.Data.Faces, MechsimOptions.Data.max_faces*sizeof (MECHSIM_FACE), "mechanics simulation face data block");
										}

										MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx1 = Idx1+prev_mass_count;
										MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx2 = Idx2+prev_mass_count;
										MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Idx3 = Idx3+prev_mass_count;

										if (Group==0) // no group given -> use those from file
											MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Grp; 
										else MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Group = Group;

										MechsimOptions.Data.Faces[MechsimOptions.Data.faces_count].Index = MechsimOptions.Data.faces_count;

										MechsimOptions.Data.faces_count++;

#if(MECHSIM_DEBUG == 1)
										Debug_Info("face load %d %d %d\n", Idx1, Idx2, Idx3);
#endif

										goodreads++;
									}
									else
									{
										Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
									}

									break;
								}    
							  case 'V':    /* Viscoelastic */
								{
									count  = sscanf(line, "V %d %d %lf %lf %d %d",
																	&Idx1, &Idx2, &Length, &Stiff, &Acc, &ElCnt);

									if ((count == 6) && (ElCnt >= 1))
									{
										if ((Idx1>=MechsimOptions.Data.mass_count) || (Idx2>=MechsimOptions.Data.mass_count))
										{
											Warning(0, "mechanics simulation data (line %d): viscoelastic index out of range, ignoring", line_num);
											continue;
										}

										if ( MechsimOptions.Data.ve_count >= MechsimOptions.Data.max_viscoelastics )
										{
											if (MechsimOptions.Data.max_viscoelastics >= INT_MAX/2)
											{
												Error("Too many viscoelastics in simulation data");
											}
											MechsimOptions.Data.max_viscoelastics *= 2;
											MechsimOptions.Data.Viscoelastics = (MECHSIM_VISCOELASTIC *)POV_REALLOC(MechsimOptions.Data.Viscoelastics, MechsimOptions.Data.max_viscoelastics*sizeof (MECHSIM_VISCOELASTIC), "mechanics simulation viscoelastic data block");
										}

										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Idx1 = Idx1+prev_mass_count;
										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Idx2 = Idx2+prev_mass_count;
										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Length = Length;
										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Stiffness0 = Stiff;
										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Accuracy = Acc;
										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].element_count = ElCnt;

										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element = 
											(MECHSIM_VE_ELEMENT *)POV_MALLOC(ElCnt*sizeof (MECHSIM_VE_ELEMENT), "mechanics simulation viscoelastic data");

										strncpy(line_param, line, MAX_STR);
										Pos = strtok(line_param, " ");              // 'V'
										if (Pos != NULL) Pos = strtok(NULL, " ");   // Index1
										if (Pos != NULL) Pos = strtok(NULL, " ");   // Index2
										if (Pos != NULL) Pos = strtok(NULL, " ");   // Length
										if (Pos != NULL) Pos = strtok(NULL, " ");   // Stiffness0
										if (Pos != NULL) Pos = strtok(NULL, " ");   // Accuracy
										if (Pos != NULL) Pos = strtok(NULL, " ");   // Element count
										if (Pos == NULL)
										{
											Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
											parse_err = true;
										}
										else for (i=0; i<ElCnt; i++)
										{
											Pos = strtok(NULL, " ");
											if (Pos != NULL) MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Y = atof(Pos);
											else 
											{
												Warning(0, "mechanics simulation data (line %d, 1): wrong syntax - skipping line", line_num);
												parse_err = true;
												break;
											}
											Pos = strtok(NULL, " ");
											if (Pos != NULL) MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Stiffness = atof(Pos);
											else 
											{
												Warning(0, "mechanics simulation data (line %d, 2): wrong syntax - skipping line", line_num);
												parse_err = true;
												break;
											}
											Pos = strtok(NULL, " ");
											if (Pos != NULL) MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Element[i].Damping = atof(Pos);
											else 
											{
												Warning(0, "mechanics simulation data (line %d, 3): wrong syntax - skipping line", line_num);
												parse_err = true;
												break;
											}

#if(MECHSIM_DEBUG == 1)
											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 );
#endif
										}

										if (parse_err)
										{
											parse_err = false;
											continue;
										}

										MechsimOptions.Data.Viscoelastics[MechsimOptions.Data.ve_count].Index = MechsimOptions.Data.ve_count;

										MechsimOptions.Data.ve_count++;

#if(MECHSIM_DEBUG == 1)
										Debug_Info("ve load %d %d %g %g %d\n", Idx1, Idx2, Length, Stiff, ElCnt);
#endif

										goodreads++;
									}
									else
									{
										Warning(0, "mechanics simulation data (line %d): wrong syntax - skipping line", line_num);
									}
									break;
								} 

							  default:
								{
									/* unknown leading character on line -> skip */
								}
								
							}   /* end switch */
						}     /* end while-reading loop */
						break;
					}
        	default:     /* other file types */
					{
						Error("mechanics simulation data format %d not supported", fformat );
						return false;
					}
				}
			}     /* end if "MSIM" */


		if ( !got_eof ) {
			Warning(0,"Error reading mechanics simulation data.");
			return false;
		}
		else
		{
			if ( goodreads > 0 )
			{
				Debug_Info("\nLoaded %d masses, %d connections, %d viscoelastics and %d faces\nfrom mechanics simulation data file %s.\n",
									 MechsimOptions.Data.mass_count,
									 MechsimOptions.Data.connect_count,
									 MechsimOptions.Data.ve_count,
									 MechsimOptions.Data.faces_count,
									 MechsimOptions.Load_File_Name);
			}
			else
			{
				Warning(0,"Unable to read any values from mechanics simulation data file.");
			}
			return true;
		}
	}
	else return false;

}

/*****************************************************************************
*
* FUNCTION
*
*   Mechsim_save_file
*
* INPUT
*   file descriptor handle of file (already opened).
*
* OUTPUT
*
* RETURNS 1 for success, 0 for failure.
*
* AUTHOUR
*
*   Christoph Hormann
*
* DESCRIPTION
*
*   Write mechanics simulation data to a file
*
* CHANGES
*
*   --- Aug 2002 : Creation
*       Feb 2005 : new file type
*       Apr 2005 : Adding Force index
*
******************************************************************************/
bool Mechsim_save_file(void)
{
	OStream *fd;
	int i, j;
	bool retval = false;
  
	fd = New_OStream(MechsimOptions.Save_File_Name, POV_File_Unknown, false);
	if ( fd == NULL)
	{
		Error("Cannot save mechanics simulation data to file %s", MechsimOptions.Save_File_Name );
		retval = false;
	}
	else
	{
		Send_Progress("Writing Mechsim data to file", PROGRESS_MECHSIM_SAVEFILE);

		fd->printf("MSIM, ");

		fd->printf("%d,\n", MechsimOptions.Save_File_Type);

		/* Info line */
		fd->printf("I %d %d %d\n", 
							 MechsimOptions.Data.mass_count, 
							 MechsimOptions.Data.connect_count, 
							 MechsimOptions.Data.faces_count, 
							 MechsimOptions.Data.ve_count);

		/* Timing line */
		fd->printf("T %g %g %d\n", 
							 MechsimOptions.Time,
							 MechsimOptions.Time_Step,
							 MechsimOptions.Step_Count);

		/* Masses */
		for (i=0; i<MechsimOptions.Data.mass_count; i++)
		{
			if ((i%100) == 0)
			{
				Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
				Do_Cooperate(0); 
			}

			fd->printf("M %.12f %.12f %.12f %.12f %.12f %.12f %g %g %d %d %d %d\n",
								 MechsimOptions.Data.Masses[i].Position[X],
								 MechsimOptions.Data.Masses[i].Position[Y],
								 MechsimOptions.Data.Masses[i].Position[Z],
								 MechsimOptions.Data.Masses[i].Velocity[X],
								 MechsimOptions.Data.Masses[i].Velocity[Y],
								 MechsimOptions.Data.Masses[i].Velocity[Z],
								 MechsimOptions.Data.Masses[i].Mass,
								 MechsimOptions.Data.Masses[i].Radius,
								 MechsimOptions.Data.Masses[i].Flag,
								 MechsimOptions.Data.Masses[i].Group,
								 MechsimOptions.Data.Masses[i].Attach,
								 MechsimOptions.Data.Masses[i].Force);
		}

		/* Connections */
		for (i=0; i<MechsimOptions.Data.connect_count; i++)
		{
			if ((i%100) == 0)
			{
				Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
				Do_Cooperate(0); 
			}
			fd->printf("C %d %d %g %g %g %d\n",
								 MechsimOptions.Data.Connects[i].Idx1,
								 MechsimOptions.Data.Connects[i].Idx2,
								 MechsimOptions.Data.Connects[i].Length,
								 MechsimOptions.Data.Connects[i].Stiffness,
								 MechsimOptions.Data.Connects[i].Damping,
								 MechsimOptions.Data.Connects[i].Group);
		}

		/* Faces */
		for (i=0; i<MechsimOptions.Data.faces_count; i++)
		{
			if ((i%100) == 0)
			{
				Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
				Do_Cooperate(0); 
			}
			fd->printf("F %d %d %d %d\n",
								 MechsimOptions.Data.Faces[i].Idx1,
								 MechsimOptions.Data.Faces[i].Idx2,
								 MechsimOptions.Data.Faces[i].Idx3,
								 MechsimOptions.Data.Faces[i].Group);
		}

		/* Viscoelastics */
		for (i=0; i<MechsimOptions.Data.ve_count; i++)
		{
			if ((i%100) == 0)
			{
				Send_ProgressUpdate(PROGRESS_MECHSIM_SAVEFILE);
				Do_Cooperate(0); 
			}
			fd->printf("V %d %d %g %g %d %d",
								 MechsimOptions.Data.Viscoelastics[i].Idx1,
								 MechsimOptions.Data.Viscoelastics[i].Idx2,
								 MechsimOptions.Data.Viscoelastics[i].Length,
								 MechsimOptions.Data.Viscoelastics[i].Stiffness0,
								 MechsimOptions.Data.Viscoelastics[i].Accuracy,
								 MechsimOptions.Data.Viscoelastics[i].element_count);

			for (j=0; j<MechsimOptions.Data.Viscoelastics[i].element_count; j++)
			{
				fd->printf(" %g %g %g",
									 MechsimOptions.Data.Viscoelastics[i].Element[j].Y,
									 MechsimOptions.Data.Viscoelastics[i].Element[j].Stiffness,
									 MechsimOptions.Data.Viscoelastics[i].Element[j].Damping);

			}
			fd->printf("\n");
		}

		fd->printf("\n");

		delete fd;

		Debug_Info("Saved simulation data to file %s\n", MechsimOptions.Save_File_Name);
 
		retval = true;
	}

	POV_FREE(MechsimOptions.Save_File_Name);
	MechsimOptions.Save_File_Name = NULL;

	return retval;
}



// ========================================================================

char *Read_Vector(IStream *fd, char *str, char *buffer, VECTOR Vect, bool *got_eof)
{
	char *Pos;
	char *End;
	char *ptr;
	char str_nbr[MAX_STR];

	if (*got_eof) return NULL;
	if (buffer == NULL) 
	{
		*got_eof = true;
		return NULL;
	}

	//Debug_Info("Read_Vector() %s\n", buffer);

	// search vector start
	Pos=strchr(buffer, '<');
	if (Pos == NULL)
	{
		// read next line when necessary
		*got_eof = fd->getline (str, MAX_STR).eof ();

		str[MAX_STR-1] = '\0';
		buffer = str;

		Pos = strchr(buffer, '\n');           // remove line break
		if (Pos != NULL) Pos[0] = '\0';
		Pos = strchr(buffer, '\r');           // remove line break
		if (Pos != NULL) Pos[0] = '\0';

		Pos=strchr(buffer, '<');
		if (Pos == NULL)
		{
			Warning(0,"Warning: strange format of mechanics simulation data (v2)");

			Vect[X] = 0.0;
			Vect[Y] = 0.0;
			Vect[Z] = 0.0;
			return buffer;
		}
	}

	if (*got_eof) return NULL;

	// read vector content
	Pos++;
	strncpy(str_nbr, Pos, MAX_STR);

	Pos = strtok(str_nbr, ", ");
	if (Pos != NULL)
	{
		Vect[X] = strtod(Pos, &End);
		Pos = strtok(NULL, ", ");
		if (Pos != NULL)
		{
			Vect[Y] = strtod(Pos, &End);
			Pos = strtok(NULL, ", ");
			if (Pos != NULL)
			{
				Vect[Z] = strtod(Pos, &End);
			}
		}
	}
	else
	{
		Warning(0,"Warning: strange format of mechanics simulation data (v1)");

		Vect[X] = 0.0;
		Vect[Y] = 0.0;
		Vect[Z] = 0.0;
		return buffer;
	}

	Pos = strchr(buffer, '>');               // search vector end
	Pos++;
	ptr = strchr(Pos, ',');
	if (ptr == NULL) ptr=strchr(Pos, ' ');
	if (ptr == NULL) ptr=strchr(Pos, '\t');

	if (ptr  == NULL)                         // no delimiter
		return Pos;

	ptr++;

	return ptr;
	
}

// ========================================================================

char *Read_Float(IStream *fd, char *str, char *buffer, double *Value, bool *got_eof)
{
	char *Pos;
	char *End;

	if (*got_eof) return NULL;
	if (buffer == NULL) 
	{
		*got_eof = true;
		return NULL;
	}

	//Debug_Info("Read_Float() %s\n", buffer);

	Pos=strchr(buffer, ',');
	if (Pos == NULL) Pos=strchr(buffer, ' ');
	if (Pos == NULL) Pos=strchr(buffer, '\t');
	if ((Pos == NULL) && (buffer[0] == '\0'))
	{
		// read next line when necessary
		*got_eof = fd->getline (str, MAX_STR).eof ();

		str[MAX_STR-1] = '\0';
		buffer = str;

		Pos = strchr(buffer, '\n');           // remove line break
		if (Pos != NULL) Pos[0] = '\0';
		Pos = strchr(buffer, '\r');           // remove line break
		if (Pos != NULL) Pos[0] = '\0';

		Pos=strchr(buffer, ',');
		if (Pos == NULL) Pos=strchr(buffer, ' ');
		if (Pos == NULL) Pos=strchr(buffer, '\t');
		if (Pos == NULL)
		{
			Warning(0,"Warning: strange format of mechanics simulation data (f)");
			return buffer;
		}
	}

	if (*got_eof) return NULL;

	Pos[0] = '\0';            // cut comma

	*Value = strtod(buffer, &End);

	Pos++;

	return Pos;

}

// ========================================================================


char *Read_Int(IStream *fd, char *str, char *buffer, int *Value, bool *got_eof)
{
	char *Pos;
	char *End;

	if (*got_eof) return NULL;
	if (buffer == NULL) 
	{
		*got_eof = true;
		return NULL;
	}

	//Debug_Info("Read_Int() %s\n", buffer);

	Pos=strchr(buffer, ',');
	if (Pos == NULL) Pos=strchr(buffer, ' ');
	if (Pos == NULL) Pos=strchr(buffer, '\t');
	if ((Pos == NULL) && (buffer[0] == '\0'))
	{
		// read next line when necessary
		*got_eof = fd->getline (str, MAX_STR).eof ();

		str[MAX_STR-1] = '\0';
		buffer = str;

		Pos = strchr(buffer, '\n');           // remove line break
		if (Pos != NULL) Pos[0] = '\0';
		Pos = strchr(buffer, '\r');           // remove line break
		if (Pos != NULL) Pos[0] = '\0';

		Pos=strchr(buffer, ',');
		if (Pos == NULL) Pos=strchr(buffer, ' ');
		if (Pos == NULL) Pos=strchr(buffer, '\t');
		if (Pos == NULL)
		{
			Warning(0,"Warning: strange format of mechanics simulation data (i)");
			return buffer;
		}
	}

	if (*got_eof) return NULL;

	Pos[0] = '\0';            // cut comma

	*Value = strtol(buffer, &End, 0);

	Pos++;

	return Pos;

}

/*****************************************************************************
*
* FUNCTION
*
*   calc_faces_bounding_box
*
* INPUT
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Calculates the bounding boxes for all faces and the unit size of the grid for faces.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/
static void calc_faces_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  SNGL_VECT Unit;
  SNGL_VECT t_max, t_min, tmp;
  int Idx1, Idx2, Idx3, i;

  Make_Vector(tmp, 0.0, 0.0, 0.0);
  for (i = 0; i < MechsimOptions.Data.faces_count; i++)
  {
    Idx1 = MechsimOptions.Data.Faces[i].Idx1;
    Idx2 = MechsimOptions.Data.Faces[i].Idx2;
    Idx3 = MechsimOptions.Data.Faces[i].Idx3;
    t_max[X] = max(Pos_Array[Idx1][X], max(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
    t_max[Y] = max(Pos_Array[Idx1][Y], max(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
    t_max[Z] = max(Pos_Array[Idx1][Z], max(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));
    t_min[X] = min(Pos_Array[Idx1][X], min(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
    t_min[Y] = min(Pos_Array[Idx1][Y], min(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
    t_min[Z] = min(Pos_Array[Idx1][Z], min(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));

    Make_BBox_from_min_max(Bounding_Data->Faces_BBox_Array[i], t_min, t_max);

    VSubEq(t_max, t_min);
    VAddEq(tmp, t_max);
  }
  VScale(Unit, tmp, 1.0/MechsimOptions.Data.faces_count);
  Assign_Vector(Bounding_Data->Unit, Unit);
}

/*****************************************************************************
*
* FUNCTION
*
*   intersect_sphere_box
*
* INPUT
*   Box bounding box.
*   Center center of sphere.
*   Radius radius of sphere.
*
* OUTPUT
*
* RETURNS true if sphere intersects box else false.
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Test if a sphere intersects a box.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/
static bool intersect_sphere_box(BBOX BBox, VECTOR Center, DBL Radius)
{
  SNGL d;
  SNGL_VECT tmp, s, SNGL_Center;

  VScale(tmp, BBox.Lengths, 0.5);
  VAdd(s, BBox.Lower_Left, tmp);
  Assign_Vector(SNGL_Center, Center);
  VSub(s, SNGL_Center, s);
  s[X] = fabs(s[X]);
  s[Y] = fabs(s[Y]);
  s[Z] = fabs(s[Z]);
  VSubEq(s, tmp);
  s[X] = max(s[X], 0.0f);
  s[Y] = max(s[Y], 0.0f);
  s[Z] = max(s[Z], 0.0f);

  d = s[X]*s[X]+s[Y]*s[Y]+s[Z]*s[Z];

  return d <= Radius*Radius;
}

/*****************************************************************************
*
* FUNCTION
*
*   calc_faces_bounding_hashtable
*
* INPUT
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Calculates the bounding hashtable for all faces.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/
static void calc_faces_bounding_hashtable(MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  long int jx_min, jy_min, jz_min;
  long int jx_max, jy_max, jz_max;
  long int jx, jy, jz;
  int i;
  unsigned int hash;

  for (i = 0; i < MechsimOptions.Data.faces_count; i++)
  {
    jx_min = (long int)floor(Bounding_Data->Faces_BBox_Array[i].Lower_Left[X]/
	  Bounding_Data->Unit[X]);
    jy_min = (long int)floor(Bounding_Data->Faces_BBox_Array[i].Lower_Left[Y]/
	  Bounding_Data->Unit[Y]);
    jz_min = (long int)floor(Bounding_Data->Faces_BBox_Array[i].Lower_Left[Z]/
	  Bounding_Data->Unit[Z]);

    jx_max = (long int)floor((Bounding_Data->Faces_BBox_Array[i].Lower_Left[X]+
	  Bounding_Data->Faces_BBox_Array[i].Lengths[X])/Bounding_Data->Unit[X]);
    jy_max = (long int)floor((Bounding_Data->Faces_BBox_Array[i].Lower_Left[Y]+
	  Bounding_Data->Faces_BBox_Array[i].Lengths[Y])/Bounding_Data->Unit[Y]);
    jz_max = (long int)floor((Bounding_Data->Faces_BBox_Array[i].Lower_Left[Z]+
	  Bounding_Data->Faces_BBox_Array[i].Lengths[Z])/Bounding_Data->Unit[Z]);

    for (jx = jx_min; jx <= jx_max; jx++)
      for (jy = jy_min; jy <= jy_max; jy++)
        for (jz = jz_min; jz <= jz_max; jz++)
        {
          hash = bounding_hash_func(jx, jy, jz, Bounding_Data->bounding_hashtable_size);
          insert_index_bounding_hastable(Bounding_Data->bounding_hashtable,
			hash, i, Bounding_Data->time_stamp);
        }
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   insert_index_bounding_hastable
*
* INPUT
*   bounding_hashtable bounding hashtable.
*   hash hash of cell.
*   index index of triangle.
*   time_stamp time stamp for faster updating of the hashtable.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Insert an index into the bounding hashtable at position hash.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/
static void insert_index_bounding_hastable(BOUNDING_HASHTABLE_ITEM* bounding_hashtable, unsigned int hash, int index, int time_stamp)
{
  int size, used;
  if ((bounding_hashtable[hash].size == 0) || (bounding_hashtable[hash].index == NULL))
  {
    size = 4;
    used = 0;
    bounding_hashtable[hash].index = (int *)POV_MALLOC(size*sizeof(int), "mechsim bounding hashtable index's");
    bounding_hashtable[hash].size = size;
  }
  else
  {
    size = bounding_hashtable[hash].size;
    if (bounding_hashtable[hash].time_stamp < time_stamp) used = 0;
    else
    {
      used = bounding_hashtable[hash].used;
      if (size < (used+1))
      {
        size *= 2;
        bounding_hashtable[hash].index = (int *)POV_REALLOC(bounding_hashtable[hash].index, size*sizeof(int),
          "mechsim bounding hashtable index's");
        bounding_hashtable[hash].size = size;
      }
    }
  }
  bounding_hashtable[hash].index[used] = index;
  bounding_hashtable[hash].time_stamp = time_stamp;
  bounding_hashtable[hash].used = used+1;
}

/*****************************************************************************
*
* FUNCTION
*
*   intersect_sphere_bounding_hashtable
*
* INPUT
*   Center center of sphere.
*   Radius radius of sphere.
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Test if a sphere intersects a bounding hashtable.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/
static void intersect_sphere_bounding_hashtable(VECTOR Center, DBL Radius, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	BBOX BBox;
	long int jx_min, jy_min, jz_min;
	long int jx_max, jy_max, jz_max;
	long int jx, jy, jz;
	unsigned int hash;
	int i;
	int idx;

	jx_min = (long int)floor((Center[X]-Radius)/Bounding_Data->Unit[X]);
	jy_min = (long int)floor((Center[Y]-Radius)/Bounding_Data->Unit[Y]);
	jz_min = (long int)floor((Center[Z]-Radius)/Bounding_Data->Unit[Z]);

	jx_max = (long int)floor((Center[X]+Radius)/Bounding_Data->Unit[X]);
	jy_max = (long int)floor((Center[Y]+Radius)/Bounding_Data->Unit[Y]);
	jz_max = (long int)floor((Center[Z]+Radius)/Bounding_Data->Unit[Z]);

	/* Kein Index wurde Kopiert, deswegen alle auf Null setzen.*/
	memset(Bounding_Data->bitfield, 0, sizeof(unsigned int)*Bounding_Data->bitfield_size);

	idx = 0;
	BBox.Lengths[X] = Bounding_Data->Unit[X];
	BBox.Lengths[Y] = Bounding_Data->Unit[Y];
	BBox.Lengths[Z] = Bounding_Data->Unit[Z];
	BBox.Lower_Left[X] = jx_min*Bounding_Data->Unit[X];
	for (jx = jx_min; jx <= jx_max; jx++)
	{
		BBox.Lower_Left[Y] = jy_min*Bounding_Data->Unit[Y];
		for (jy = jy_min; jy <= jy_max; jy++)
		{
			BBox.Lower_Left[Z] = jz_min*Bounding_Data->Unit[Z];
			for (jz = jz_min; jz <= jz_max; jz++)
			{
				/* We test if the sphere intersects this bounding box */
				if (intersect_sphere_box(BBox, Center, Radius))
				{
					hash = bounding_hash_func(jx, jy, jz, Bounding_Data->bounding_hashtable_size);
					/* Ist der Eintrag überhaupt aktuell? */
					if (Bounding_Data->bounding_hashtable[hash].time_stamp == Bounding_Data->time_stamp)
					{
						/* Brauchen wir vieleicht mehr Speicher? */
						if (Bounding_Data->intersect_list_size < (idx +	Bounding_Data->bounding_hashtable[hash].used))
						{
							Bounding_Data->intersect_list_size += Bounding_Data->bounding_hashtable[hash].used;
							Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list,
							  Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding hashtable intersect list");
						}
						/* Kopieren der Indizies. */	
						for (i = 0; i < Bounding_Data->bounding_hashtable[hash].used; i++)
						{
							/* Nur Kopieren, was nicht schon da ist. */
							if (!testbit(Bounding_Data->bitfield, Bounding_Data->bounding_hashtable[hash].index[i]))
							{
								Bounding_Data->intersect_list[idx] = Bounding_Data->bounding_hashtable[hash].index[i];
								idx++;
								/* Dieser Index ist nun Kopiert, deswegen wird das entsprechende Bit gesetzt. */
								setbit(Bounding_Data->bitfield, Bounding_Data->bounding_hashtable[hash].index[i]);
							}
						}
					}
				}
				BBox.Lower_Left[Z] += Bounding_Data->Unit[Z];
			}
			BBox.Lower_Left[Y] += Bounding_Data->Unit[Y];
		}
		BBox.Lower_Left[X] += Bounding_Data->Unit[X];
	}

	Bounding_Data->intersect_list_index = idx;
}

/*****************************************************************************
*
* FUNCTION
*
*   calc_face_groups_bounding_box
*
* INPUT
*   Pos_Array array of masses positions.
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Berechnet die Bounding-Boxen für alle Gruppen. Dabei werden nur die Seiten berücksichtigt.
*
* CHANGES
*
*   --- April 2004 : Creation
*
******************************************************************************/
static void calc_face_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  VECTOR t_max, t_min, g_max, g_min;
  int Idx1, Idx2, Idx3, i, j, Index;

  for (i = 0; i < MechsimOptions.Data.group_count; i++)
  {
    Make_Vector(g_max, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
    /* Die Gruppen enthält keine Seiten, also kann die Bounding-Box die Größe Null haben und möglichst weit weg liegen. */
    if (MechsimOptions.Data.Groups[i].faces_count == 0)
    {
      Make_Vector(g_min, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
      Make_BBox_from_min_max(Bounding_Data->Face_Groups_BBox_Array[i], g_min, g_max);
    }
    else
    {
      Make_Vector(g_min,  BOUND_HUGE,  BOUND_HUGE,  BOUND_HUGE);

      for (j = 0; j < MechsimOptions.Data.Groups[i].faces_count; j++)
      {
        Index = MechsimOptions.Data.Groups[i].Faces[j]->Index;
        Idx1 = MechsimOptions.Data.Groups[i].Faces[j]->Idx1;
        Idx2 = MechsimOptions.Data.Groups[i].Faces[j]->Idx2;
        Idx3 = MechsimOptions.Data.Groups[i].Faces[j]->Idx3;
        t_max[X] = max(Pos_Array[Idx1][X], max(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
        t_max[Y] = max(Pos_Array[Idx1][Y], max(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
        t_max[Z] = max(Pos_Array[Idx1][Z], max(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));
        t_min[X] = min(Pos_Array[Idx1][X], min(Pos_Array[Idx2][X], Pos_Array[Idx3][X]));
        t_min[Y] = min(Pos_Array[Idx1][Y], min(Pos_Array[Idx2][Y], Pos_Array[Idx3][Y]));
        t_min[Z] = min(Pos_Array[Idx1][Z], min(Pos_Array[Idx2][Z], Pos_Array[Idx3][Z]));

        /* Bounding-Box der Seite. */
        Make_BBox_from_min_max(Bounding_Data->Faces_BBox_Array[Index], t_min, t_max);

        g_max[X] = max(g_max[X], t_max[X]);
        g_max[Y] = max(g_max[Y], t_max[Y]);
        g_max[Z] = max(g_max[Z], t_max[Z]);
        g_min[X] = min(g_min[X], t_min[X]);
        g_min[Y] = min(g_min[Y], t_min[Y]);
        g_min[Z] = min(g_min[Z], t_min[Z]);
      }
      /* Bounding-Box der Gruppe. */
      Make_BBox_from_min_max(Bounding_Data->Face_Groups_BBox_Array[i], g_min, g_max);
    }
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   intersect_box_box
*
* INPUT
*   BBox_1 box number one.
*   BBox_2 box number two.
*
* OUTPUT
*
* RETURNS true if box one intersects box two else false.
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Gibt true zurück, wenn die zwei Quader sich berühre oder eine Schnittmenge
*   besitzen, die nicht leer ist.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/

static bool intersect_box_box(BBOX BBox_1, BBOX BBox_2)
{
  SNGL_VECT tmp1, tmp2, tmp3;
  VAdd(tmp1, BBox_1.Lower_Left, BBox_1.Lengths);
  VAdd(tmp2, BBox_2.Lower_Left, BBox_2.Lengths);
  tmp3[X] = max(BBox_1.Lower_Left[X], BBox_2.Lower_Left[X]);
  tmp3[Y] = max(BBox_1.Lower_Left[Y], BBox_2.Lower_Left[Y]);
  tmp3[Z] = max(BBox_1.Lower_Left[Z], BBox_2.Lower_Left[Z]);
  tmp1[X] = min(tmp1[X], tmp2[X]);
  tmp1[Y] = min(tmp1[Y], tmp2[Y]);
  tmp1[Z] = min(tmp1[Z], tmp2[Z]);
  return ((tmp3[X] <= tmp1[X]) && (tmp3[Y] <= tmp1[Y]) && (tmp3[Z] <= tmp1[Z]));
}

/*****************************************************************************
*   Berechnet welche Seiten in der Schnittmenge liegen. Nur die Seiten, die
*   tatsächlich geschnitten werden, müssen später überprüft werde.
******************************************************************************/
static void calc_face_group_intersection(BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  int i, Idx, Index;

  if (Bounding_Data->intersect_list_size < MechsimOptions.Data.Groups[Grp].faces_count)
  {
    Bounding_Data->intersect_list_size = MechsimOptions.Data.Groups[Grp].faces_count;
    Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list,
      Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding box intersect list");
  }
  Index = 0;
  for (i = 0; i < MechsimOptions.Data.Groups[Grp].faces_count; i++)
  {
    Idx = MechsimOptions.Data.Groups[Grp].Faces[i]->Index;
    if (intersect_box_box(intersection_Box, Bounding_Data->Faces_BBox_Array[Idx]))
    {
      Bounding_Data->intersect_list[Index] = Idx;
      Index++;
    }
  }
  Bounding_Data->intersect_list_index = Index;
}

/*****************************************************************************
*
* FUNCTION
*
*   init_bounding
*
* INPUT
*
* OUTPUT
*   Bounding_Data all bounding data.
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Initialisiert alle Boundingdaten.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/

static MECHSIM_BOUNDING_DATA* init_bounding(void)
{
  MECHSIM_BOUNDING_DATA* Bounding_Data = NULL;
	
  /* Initialization of bounding */
  if (MechsimOptions.Bounding != MECHSIM_FACE_BOUNDING_NO)
  {
    Bounding_Data = (MECHSIM_BOUNDING_DATA *)POV_MALLOC(sizeof(MECHSIM_BOUNDING_DATA), "mechsim bounding data");
    memset(Bounding_Data, 0, sizeof(MECHSIM_BOUNDING_DATA));

    Bounding_Data->intersect_list_size = 8;
    Bounding_Data->intersect_list_index = 0;
	
    if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
    {
      Bounding_Data->bounding_hashtable_size = MechsimOptions.Data.faces_count;
      Bounding_Data->bounding_hashtable = (BOUNDING_HASHTABLE_ITEM*)POV_MALLOC(Bounding_Data->bounding_hashtable_size
        *sizeof(BOUNDING_HASHTABLE_ITEM), "mechsim faces bounding hashtable");
      memset(Bounding_Data->bounding_hashtable, 0, Bounding_Data->bounding_hashtable_size*sizeof(BOUNDING_HASHTABLE_ITEM));
	
      Bounding_Data->time_stamp = 1;
	
      Bounding_Data->bitfield_size = MechsimOptions.Data.faces_count/BITSPERINT+1;
      Bounding_Data->bitfield = (unsigned int *)POV_MALLOC(Bounding_Data->bitfield_size*sizeof(unsigned int),
	    "mechsim bounding hashtable bitfield");
	
      Bounding_Data->intersect_list = (int *)POV_MALLOC(Bounding_Data->intersect_list_size*sizeof(int),
	    "mechsim bounding hashtable intersect list");

    }
    else
    {
      Bounding_Data->intersect_list = (int *)POV_MALLOC(Bounding_Data->intersect_list_size*sizeof(int),
	  "mechsim bounding box intersect list");
    }

    Bounding_Data->Faces_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_faces*sizeof(BBOX),
      "mechsim faces bounding boxes");
    Bounding_Data->Face_Groups_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof(BBOX),
      "mechsim face groups bounding boxes");
    Bounding_Data->Mass_Groups_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof(BBOX),
      "mechsim mass groups bounding boxes");
    Bounding_Data->Connect_Groups_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_groups*sizeof(BBOX),
      "mechsim connections groups bounding boxes");
    Bounding_Data->Connects_BBox_Array = (BBOX *)POV_MALLOC(MechsimOptions.Data.max_connects*sizeof(BBOX),
      "mechsim connections bounding boxes");
  }
  return Bounding_Data;
}

/*****************************************************************************
*
* FUNCTION
*
*   free_bounding
*
* INPUT
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Gibt alle Boundingdaten frei.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/

static void free_bounding(MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	int i;
	
	/* Deinitialization of bounding */
	if (MechsimOptions.Bounding >= MECHSIM_FACE_BOUNDING_BBOX)
	{
		if (Bounding_Data->intersect_list != NULL) POV_FREE(Bounding_Data->intersect_list);
		if (MechsimOptions.Bounding == MECHSIM_FACE_BOUNDING_HASH)
		{
			if (Bounding_Data->bounding_hashtable != NULL)
			{
				for (i = 0; i < Bounding_Data->bounding_hashtable_size; i++)
				{
					if (Bounding_Data->bounding_hashtable[i].index != NULL)
					{
						POV_FREE(Bounding_Data->bounding_hashtable[i].index);
					}
				}
				POV_FREE(Bounding_Data->bounding_hashtable);
			}
			if (Bounding_Data->bitfield != NULL) POV_FREE(Bounding_Data->bitfield);
		}
		if (Bounding_Data->Faces_BBox_Array != NULL) POV_FREE(Bounding_Data->Faces_BBox_Array);
		if (Bounding_Data->Face_Groups_BBox_Array != NULL) POV_FREE(Bounding_Data->Face_Groups_BBox_Array);
		if (Bounding_Data->Mass_Groups_BBox_Array != NULL) POV_FREE(Bounding_Data->Mass_Groups_BBox_Array);
		if (Bounding_Data->Connect_Groups_BBox_Array != NULL) POV_FREE(Bounding_Data->Connect_Groups_BBox_Array);
		if (Bounding_Data->Connects_BBox_Array != NULL) POV_FREE(Bounding_Data->Connects_BBox_Array);
		POV_FREE(Bounding_Data);
	}
}

/*****************************************************************************
*
* FUNCTION
*
*   calc_mass_unit_size
*
* INPUT
*
* OUTPUT
*
* RETURNS Unit size.
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Berechnet die Gittergröße für die Hash-Tabelle als das Doppeltes des 
*   durchschnittlichen Massenradius
*
* CHANGES
*
*   --- April 2004 : Creation
*
******************************************************************************/

static SNGL calc_mass_unit_size(void)
{
  int i;
  SNGL Unit;
  
  Unit = 0.0;
  
  for (i = 0; i < MechsimOptions.Data.mass_count; i++)
    Unit += MechsimOptions.Data.Masses[i].Radius;
    
  Unit *= 2;
  
  Unit /= MechsimOptions.Data.mass_count;
  return Unit;
}

/*****************************************************************************
*
* FUNCTION
*
*   calc_masses_bounding_hashtable
*
* INPUT
*   Pos_Array array of masses positions.
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS bounding hashtable for all masses.
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Calculates the bounding hashtable for all masses.
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/

static void calc_masses_bounding_hashtable(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  BBOX BBox;
  long int jx_min, jy_min, jz_min;
  long int jx_max, jy_max, jz_max;
  long int jx, jy, jz;
  int i, hash;
  
  for (i = 0; i < MechsimOptions.Data.mass_count; i++)
  {
    /* Bestimmung des minimalen Gitterpunktes der Kugel */
    jx_min = (long int)floor((Pos_Array[i][X]-MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[X]);
    jy_min = (long int)floor((Pos_Array[i][Y]-MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Y]);
    jz_min = (long int)floor((Pos_Array[i][Z]-MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Z]);

    /* Bestimmung des maximalen Gitterpunktes der Kugel */
    jx_max = (long int)floor((Pos_Array[i][X]+MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[X]);
    jy_max = (long int)floor((Pos_Array[i][Y]+MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Y]);
    jz_max = (long int)floor((Pos_Array[i][Z]+MechsimOptions.Data.Masses[i].Radius)/Bounding_Data->Unit[Z]);

    BBox.Lengths[X] = Bounding_Data->Unit[X];
    BBox.Lengths[Y] = Bounding_Data->Unit[Y];
    BBox.Lengths[Z] = Bounding_Data->Unit[Z];
    BBox.Lower_Left[X] = jx_min*Bounding_Data->Unit[X];
    for (jx = jx_min; jx <= jx_max; jx++)
    {
      BBox.Lower_Left[Y] = jy_min*Bounding_Data->Unit[Y];
      for (jy = jy_min; jy <= jy_max; jy++)
      {
        BBox.Lower_Left[Z] = jz_min*Bounding_Data->Unit[Z];
        for (jz = jz_min; jz <= jz_max; jz++)
        {
	  /* We test if the sphere intersects this bounding box */
  	  if (intersect_sphere_box(BBox, Pos_Array[i], MechsimOptions.Data.Masses[i].Radius))
	  {
	    hash = bounding_hash_func(jx, jy, jz, Bounding_Data->bounding_hashtable_size);
	    insert_index_bounding_hastable(Bounding_Data->bounding_hashtable, hash, i, Bounding_Data->time_stamp);
	  }	
          BBox.Lower_Left[Z] += Bounding_Data->Unit[Z];
        }
        BBox.Lower_Left[Y] += Bounding_Data->Unit[Y];
      }
      BBox.Lower_Left[X] += Bounding_Data->Unit[X];
    }
  }
}

/*****************************************************************************
*
* FUNCTION
*
*   calc_mass_groups_bounding_box
*
* INPUT
*   Pos_Array array of masses positions.
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS 
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Berechnet die Bounding-Boxen für alle Gruppen. Dabei werden nur die Massen berücksichtigt.
*
* CHANGES
*
*   --- April 2004 : Creation
*
******************************************************************************/

static void calc_mass_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
{ 
  VECTOR t_max, t_min, g_max, g_min;
  int Idx, i, j;
  
  for (i = 0; i < MechsimOptions.Data.group_count; i++)
  {
    Make_Vector(g_max, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
    /* Die Gruppen enthält keine Massen, also kann die Bounding-Box die Größe Null haben und möglichst weit weg liegen. */
    if (MechsimOptions.Data.Groups[i].mass_count == 0)
    {
      Make_Vector(g_min, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
      Make_BBox_from_min_max(Bounding_Data->Mass_Groups_BBox_Array[i], g_min, g_max);
    }
    else
    {
      Make_Vector(g_min,  BOUND_HUGE,  BOUND_HUGE,  BOUND_HUGE);
    
      for (j = 0; j < MechsimOptions.Data.Groups[i].mass_count; j++)
      {
        Idx = MechsimOptions.Data.Groups[i].Masses[j]->Index;
        t_max[X] = Pos_Array[Idx][X] + MechsimOptions.Data.Masses[Idx].Radius;
        t_max[Y] = Pos_Array[Idx][Y] + MechsimOptions.Data.Masses[Idx].Radius;
        t_max[Z] = Pos_Array[Idx][Z] + MechsimOptions.Data.Masses[Idx].Radius;
        t_min[X] = Pos_Array[Idx][X] - MechsimOptions.Data.Masses[Idx].Radius;
        t_min[Y] = Pos_Array[Idx][Y] - MechsimOptions.Data.Masses[Idx].Radius;
        t_min[Z] = Pos_Array[Idx][Z] - MechsimOptions.Data.Masses[Idx].Radius;
      
        g_max[X] = max(g_max[X], t_max[X]);
        g_max[Y] = max(g_max[Y], t_max[Y]);
        g_max[Z] = max(g_max[Z], t_max[Z]);      
        g_min[X] = min(g_min[X], t_min[X]);
        g_min[Y] = min(g_min[Y], t_min[Y]);
        g_min[Z] = min(g_min[Z], t_min[Z]);
      }
      /* Bounding-Box der Gruppe. */
      Make_BBox_from_min_max(Bounding_Data->Mass_Groups_BBox_Array[i], g_min, g_max);      
    }
  }
}

/*****************************************************************************
*   Berechnet welche Massen in der Schnittmenge liegen. Nur die Massen, die 
*   tatsächlich geschnitten werden, müssen später überprüft werde.
******************************************************************************/
static void calc_mass_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  int i, Idx, Index;
  
  if (Bounding_Data->intersect_list_size < MechsimOptions.Data.Groups[Grp].mass_count)
  {
    Bounding_Data->intersect_list_size = MechsimOptions.Data.Groups[Grp].mass_count;
    Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list, 
      Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding box intersect list"); 
  }
  Index = 0;
  for (i = 0; i < MechsimOptions.Data.Groups[Grp].mass_count; i++)
  {
    Idx = MechsimOptions.Data.Groups[Grp].Masses[i]->Index;
    if (intersect_sphere_box(intersection_Box, Pos_Array[Idx], MechsimOptions.Data.Masses[Idx].Radius))
    {
      Bounding_Data->intersect_list[Index] = Idx;
      Index++;
    }
  }
  Bounding_Data->intersect_list_index = Index;
}

/*****************************************************************************
*
* FUNCTION
*
*   calc_connect_groups_bounding_box
*
* INPUT
*   Pos_Array array of masses positions.
*   Bounding_Data all bounding data.
*
* OUTPUT
*
* RETURNS
*
* AUTHOUR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   Berechnet die Bounding-Boxen für alle Gruppen. Dabei werden nur die Verbindungen berücksichtigt.
*
* CHANGES
*
*   --- Sep 2004 : Creation
*
******************************************************************************/
static void calc_connect_groups_bounding_box(VECTOR* Pos_Array, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
	VECTOR t_max, t_min, g_max, g_min;
	SNGL_VECT Basis[3], tmp_vect, tmp_vect1, tmp_vect2;
	SNGL tmp, Radius;
	int Idx1, Idx2, Index, i, j;
	int Dir1, Dir2, Dir3;

	for (i = 0; i < MechsimOptions.Data.group_count; i++)
	{
		Make_Vector(g_max, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
		/* Die Gruppen enthält keine Verbindungen, also kann die Bounding-Box die GröÃe Null haben und möglichst weit weg liegen. */
		if (MechsimOptions.Data.Groups[i].connect_count == 0)
		{
			Make_Vector(g_min, -BOUND_HUGE, -BOUND_HUGE, -BOUND_HUGE);
			Make_BBox_from_min_max(Bounding_Data->Connect_Groups_BBox_Array[i], g_min, g_max);
		}
		else
		{
			Make_Vector(g_min,  BOUND_HUGE,  BOUND_HUGE,  BOUND_HUGE);

			for (j = 0; j < MechsimOptions.Data.Groups[i].connect_count; j++)
			{
				Index = MechsimOptions.Data.Groups[i].Connects[j]->Index;
				Idx1 = MechsimOptions.Data.Groups[i].Connects[j]->Idx1;
				Idx2 = MechsimOptions.Data.Groups[i].Connects[j]->Idx2;
				Radius = (MechsimOptions.Data.Masses[Idx1].Radius + MechsimOptions.Data.Masses[Idx2].Radius) / 2.0;  
				t_max[X] = max(Pos_Array[Idx1][X], Pos_Array[Idx2][X]) + Radius;
				t_max[Y] = max(Pos_Array[Idx1][Y], Pos_Array[Idx2][Y]) + Radius;
				t_max[Z] = max(Pos_Array[Idx1][Z], Pos_Array[Idx2][Z]) + Radius;
				t_min[X] = min(Pos_Array[Idx1][X], Pos_Array[Idx2][X]) - Radius;
				t_min[Y] = min(Pos_Array[Idx1][Y], Pos_Array[Idx2][Y]) - Radius;
				t_min[Z] = min(Pos_Array[Idx1][Z], Pos_Array[Idx2][Z]) - Radius;
				Make_BBox_from_min_max(Bounding_Data->Connects_BBox_Array[Index], t_min, t_max);
      
				g_max[X] = max(g_max[X], t_max[X]);
				g_max[Y] = max(g_max[Y], t_max[Y]);
				g_max[Z] = max(g_max[Z], t_max[Z]);      
				g_min[X] = min(g_min[X], t_min[X]);
				g_min[Y] = min(g_min[Y], t_min[Y]);
				g_min[Z] = min(g_min[Z], t_min[Z]);
			}
			/* Bounding-Box der Gruppe. */
			Make_BBox_from_min_max(Bounding_Data->Connect_Groups_BBox_Array[i], g_min, g_max);
		}
	}
}


/*****************************************************************************
*   Berechnet welche Verbindungen in der Schnittmenge liegen. Nur die Verbindungen, 
*   die tatsächlich geschnitten werden, müssen später überprüft werde.
******************************************************************************/
static void calc_connect_group_intersection(VECTOR *Pos_Array, BBOX intersection_Box, int Grp, MECHSIM_BOUNDING_DATA* Bounding_Data)
{
  int i, Idx, Index;	
  
  if (Bounding_Data->intersect_list_size < MechsimOptions.Data.Groups[Grp].connect_count)
  {
    Bounding_Data->intersect_list_size = MechsimOptions.Data.Groups[Grp].connect_count;
    Bounding_Data->intersect_list = (int *)POV_REALLOC(Bounding_Data->intersect_list, 
      Bounding_Data->intersect_list_size*sizeof(int), "mechsim bounding box intersect list"); 
  }
  Index = 0;
  for (i = 0; i < MechsimOptions.Data.Groups[Grp].connect_count; i++)
  {
    Idx = MechsimOptions.Data.Groups[Grp].Connects[i]->Index;
    if (intersect_box_box(intersection_Box, Bounding_Data->Connects_BBox_Array[Idx]))
    {
      Bounding_Data->intersect_list[Index] = Idx;
      Index++;
    }
  }
  Bounding_Data->intersect_list_index = Index;
}

/*****************************************************************************
* Inline functions
******************************************************************************/

/*****************************************************************************
*
* FUNCTION
*
*   bounding_hash_func
*
* INPUT
*
* OUTPUT
*
* RETURNS
*
*   int - hash value of the position
*
* AUTHOR
*
*   Daniel Jungmann
*
* DESCRIPTION
*
*   -
*
* CHANGES
*
*   --- Mar 2004 : Creation
*
******************************************************************************/

inline unsigned int bounding_hash_func(long int x, long int y, long int z, long int n)
{
  long int r;
  unsigned int ret;
  x *= 73856093L;
  y *= 19349663L;
  z *= 83492791L;
  r = x ^ y ^ z;
  r %= n;
  ret = abs(r);
  return ret;
}

// ========================================================================

END_POV_NAMESPACE

#endif