New Hardware RET6 code for more channels, added particle elements, physics.

balance_maple/balance_maple.pde

@@ -1,96 +0,0 @@
-/*
- Balance Platform code for Maple ret6
-
- Read a number of piezo pressure sensors at analog inputs and send data to PC 
- 
-*/
-
-const int inputPinX = 0;
-const int inputPinY = 1;
-const int inputPinZ = 2;
-const int inputPinW = 3;
-
-int pingRcvd = 0; 
-
-int corners[4];
-int baseline[4];
-
-float accum[4];
-
-int sampleCount = 0;
-const int NUM_SAMPLES=100;
-
-const int debug = 1; 
-
-unsigned int time;
-
-char readBuffer[100]; 
-
-void setup()
-{
-   pinMode(inputPinX, INPUT_ANALOG);
-   pinMode(inputPinY, INPUT_ANALOG);
-   pinMode(inputPinZ, INPUT_ANALOG);
-   pinMode(inputPinW, INPUT_ANALOG);
-   pinMode(BOARD_LED_PIN, OUTPUT);
-   
-   corners[0] = corners[1] = corners[2] = corners[3] = 0;
-   accum[0] = accum[1] = accum[2] = accum[3] = 0.f;
-   
-   baseline[0] = analogRead(inputPinX);
-   baseline[1] = analogRead(inputPinY);
-   baseline[2] = analogRead(inputPinZ);
-}
-
-void loop()
-{
-   sampleCount++; 
-   //  Read the instantaneous value of the pressure sensors, average over last 10 samples
-   accum[0] += analogRead(inputPinX);
-   accum[1] += analogRead(inputPinY);
-   accum[2] += analogRead(inputPinZ);
-   accum[3] += analogRead(inputPinW);
-   //accum[2] += analogRead(inputPinZ);
-  //corners[0] = accum[0];
-  //corners[1] = accum[1];
-  //corners[2] = accum[2];
-  
-   //  Periodically send averaged value to the PC
-   //  Print out the instantaneous deviation from the trailing average
-   if (sampleCount % NUM_SAMPLES == 0)
-   {
-     corners[0] = accum[0] / NUM_SAMPLES;
-     corners[1] = accum[1] / NUM_SAMPLES;
-     corners[2] = accum[2] / NUM_SAMPLES;
-     corners[3] = accum[3] / NUM_SAMPLES;
-     //corners[3] = accum[3] / NUM_SAMPLES;
-     accum[0] = accum[1] = accum[2] = accum[3] = 0.f;
-     
-      if (debug) 
-      {
-        //SerialUSB.print("Measured = ");
-        SerialUSB.print(corners[0]);
-        SerialUSB.print(" ");
-        SerialUSB.print(corners[1]);
-        SerialUSB.print(" ");
-        SerialUSB.print(corners[2]);
-        SerialUSB.print(" ");
-        SerialUSB.print(corners[3]);
-        SerialUSB.println("");
-      }
-   }
-   pingRcvd = 0;
-   while (SerialUSB.available() > 0)
-   {
-       pingRcvd = 1;
-       readBuffer[0] = SerialUSB.read();
-   }
-   if (pingRcvd == 1) 
-   {
-       SerialUSB.println("pong");
-       toggleLED();
-   }
-}
-
-
-  

field.cpp

@@ -6,21 +6,13 @@
 //  Copyright (c) 2012 __MyCompanyName__. All rights reserved.
 //
 
-#ifdef __APPLE__
-#include <GLUT/glut.h>
-#else
-#include <GL/glut.h>
-#endif
-#include <iostream>
 #include "field.h"
-#include "world.h"
+#define FIELD_SCALE 0.00050
 
 //  A vector-valued field over an array of elements arranged as a 3D lattice 
 
 struct {
-    float x;
+    glm::vec3 val;
-    float y;
-    float z;
 } field[FIELD_ELEMENTS];
 
 
@@ -33,24 +25,24 @@ int field_value(float *value, float *pos)
     (int)(pos[2]/WORLD_SIZE*10.0)*100;
     if ((index >= 0) && (index < FIELD_ELEMENTS))
     {
-        value[0] = field[index].x;
+        value[0] = field[index].val.x;
-        value[1] = field[index].y;
+        value[1] = field[index].val.y;
-        value[2] = field[index].z;
+        value[2] = field[index].val.z;
         return 1;
     }
     else return 0;
 }
 
+
 void field_init()
 //  Initializes the field to some random values
 {
     int i;
-    const float FIELD_SCALE = 0.00050;
     for (i = 0; i < FIELD_ELEMENTS; i++)
     {
-        field[i].x = (randFloat() - 0.5)*FIELD_SCALE;
+        field[i].val.x = (randFloat() - 0.5)*FIELD_SCALE;
-        field[i].y = (randFloat() - 0.5)*FIELD_SCALE;
+        field[i].val.y = (randFloat() - 0.5)*FIELD_SCALE;
-        field[i].z = (randFloat() - 0.5)*FIELD_SCALE;
+        field[i].val.z = (randFloat() - 0.5)*FIELD_SCALE;
     }       
 }
 
@@ -62,12 +54,66 @@ void field_add(float* add, float *pos)
     (int)(pos[2]/WORLD_SIZE*10.0)*100;
     if ((index >= 0) && (index < FIELD_ELEMENTS))
     {
-        field[index].x += add[0];
+        field[index].val.x += add[0];
-        field[index].y += add[0];
+        field[index].val.y += add[0];
-        field[index].z += add[0];
+        field[index].val.z += add[0];
     }
 }
 
+void field_avg_neighbors(int index, glm::vec3 * result) {
+    //  Given index to field element i, return neighbor field values 
+    glm::vec3 neighbors(0,0,0);
+    
+    int x,y,z;
+    x = (int)(index % 10);
+    y = (int)(index%100 / 10);
+    z = (int)(index / 100);
+
+    neighbors += field[(x+1)%10 + y*10 + z*100].val;
+    neighbors += field[(x-1)%10 + y*10 + z*100].val;
+    
+    neighbors += field[x + ((y+1)%10)*10 + z*100].val;    
+    neighbors += field[x + ((y-1)%10)*10 + z*100].val;  
+    
+    neighbors += field[x + y*10 + ((z+1)%10)*100].val;    
+    neighbors += field[x%10 + y*10 + ((z-1)%10)*100].val;  
+    
+    neighbors /= 6;
+    result->x = neighbors.x;
+    result->y = neighbors.y;
+    result->z = neighbors.z;
+    
+}
+
+void field_simulate(float dt) {
+    glm::vec3 neighbors, add;
+    float size;
+    for (int i = 0; i < FIELD_ELEMENTS; i++)
+    {
+        if (0) { //(randFloat() > 0.01) {
+            field_avg_neighbors(i, &neighbors);
+            size = powf(field[i].val.x*field[i].val.x +
+                        field[i].val.y*field[i].val.y + 
+                        field[i].val.z*field[i].val.z, 0.5);
+            
+            neighbors *= 0.0001;
+            glm::vec3 test = glm::normalize(glm::vec3(0,0,0));
+            
+            field[i].val = glm::normalize(field[i].val);
+            field[i].val *= size * 0.99;
+            add = glm::normalize(neighbors); 
+            add *= size * 0.01;
+            field[i].val += add;
+        }
+        else {
+            field[i].val.x += (randFloat() - 0.5)*0.01*FIELD_SCALE;
+            field[i].val.y += (randFloat() - 0.5)*0.01*FIELD_SCALE;
+            field[i].val.z += (randFloat() - 0.5)*0.01*FIELD_SCALE;
+        }
+        
+    }      
+}
+
 void field_render()
 //  Render the field lines
 {
@@ -75,6 +121,7 @@ void field_render()
     float fx, fy, fz;
     float scale_view = 1000.0;
     
+    glDisable(GL_LIGHTING);
     glColor3f(0, 1, 0);
     glBegin(GL_LINES);
     for (i = 0; i < FIELD_ELEMENTS; i++)
@@ -84,9 +131,9 @@ void field_render()
         fz = (int)(i / 100);
         
         glVertex3f(fx, fy, fz);
-        glVertex3f(fx + field[i].x*scale_view, 
+        glVertex3f(fx + field[i].val.x*scale_view, 
-                   fy + field[i].y*scale_view, 
+                   fy + field[i].val.y*scale_view, 
-                   fz + field[i].z*scale_view);
+                   fz + field[i].val.z*scale_view);
         
     }
     glEnd();

field.h

@@ -9,6 +9,14 @@
 #ifndef interface_field_h
 #define interface_field_h
 
+#ifdef __APPLE__
+#include <GLUT/glut.h>
+#else
+#include <GL/glut.h>
+#endif
+#include <iostream>
+#include "world.h"
+#include "util.h"
 #include "glm/glm.hpp"
 
 //  Field is a lattice of vectors uniformly distributed FIELD_ELEMENTS^(1/3) on side 
@@ -19,6 +27,7 @@ void field_init();
 int field_value(float *ret, float *pos);
 void field_render();
 void field_add(float* add, float *loc);
+void field_simulate(float dt);
 
 class Field {
 public:

hand.cpp

@@ -12,8 +12,10 @@ const float DEFAULT_X = 0.0;
 const float DEFAULT_Y = 0.0; 
 const float DEFAULT_Z = -7.0; 
 
-Hand::Hand()
+Hand::Hand(float initradius, glm::vec3 initcolor)
 {
+    color = initcolor;
+    radius = initradius;
     reset();
     noise = 0;
 }
@@ -23,7 +25,8 @@ void Hand::render()
     glEnable(GL_DEPTH_TEST);
     glPushMatrix();
     glLoadIdentity();
-    glColor3f(0.5, 0.5, 0.5);
+    if (isColliding) glColor3f(1,0,0);
+    else glColor3f(color.x, color.y, color.z);
     glBegin(GL_LINES);
         glVertex3f(-0.05, -0.5, 0.0);
         glVertex3f(position.x, position.y, position.z);
@@ -31,7 +34,7 @@ void Hand::render()
         glVertex3f(position.x, position.y, position.z);
     glEnd();
     glTranslatef(position.x, position.y, position.z);
-    glutSolidSphere(0.2, 15, 15);
+    glutSolidSphere(radius, 15, 15);
     glPopMatrix();
 }
 
@@ -41,6 +44,7 @@ void Hand::reset()
     position.y = DEFAULT_Y;
     position.z = DEFAULT_Z;
     velocity.x = velocity.y = velocity.z = 0;
+    isColliding = false;
 }
 
 void Hand::simulate(float deltaTime)

hand.h

@@ -20,15 +20,22 @@ const float RADIUS_RANGE = 10.0;
 
 class Hand {
 public:
-    Hand(void);
+    Hand(float initradius, glm::vec3 color);
     void simulate (float deltaTime);
     void render ();
     void reset ();
     void setNoise (float mag) { noise = mag; };
     void addVel (glm::vec3 add) { velocity += add; }; 
+    glm::vec3 getPos() { return position; };
+    void setPos(glm::vec3 newpos) { position = newpos; };
+    float getRadius() { return radius; };
+    void  setRadius(float newradius) { radius = newradius; };
+    void setColliding(bool newcollide) { isColliding = newcollide; };
 private:
-    glm::vec3 position, velocity;
+    glm::vec3 position, velocity, color;
     float noise;
+    float radius;
+    bool isColliding;
     
 };
 

hardware/head_hand/head_hand.pde

@@ -0,0 +1,44 @@
+/*
+Read a set of analog input lines and echo their readings over the serial port with averaging 
+*/
+
+#define NUM_CHANNELS 8
+#define AVERAGE_COUNT 100
+
+int inputPins[NUM_CHANNELS] = {0,1,2,3,4,10,11,12};
+
+int measured[NUM_CHANNELS];
+float accumulate[NUM_CHANNELS];
+
+int sampleCount = 0;
+
+void setup()
+{
+  int i;
+  for (i = 0; i < NUM_CHANNELS; i++) {
+    pinMode(inputPins[i], INPUT_ANALOG);
+    measured[i] = analogRead(inputPins[i]);
+    accumulate[i] = measured[i];
+  }
+  pinMode(BOARD_LED_PIN, OUTPUT);
+}
+
+void loop()
+{
+  int i;
+  sampleCount++; 
+  for (i = 0; i < NUM_CHANNELS; i++) {
+    if (sampleCount % NUM_SAMPLES == 0) {
+      measured[i] = accumulate[i] / AVERAGE_COUNT;
+      SerialUSB.print(measured[i]);
+      SerialUSB.print(" ");
+      accumulate[i] = 0;
+    } else {
+      accumulate[i] += analogRead(inputPins[i]);
+    }
+  }
+  if (sampleCount % NUM_SAMPLES == 0) SerialUSB.println("");
+}
+
+
+  

head.cpp

@@ -48,6 +48,7 @@ Head::Head()
 
 void Head::reset()
 {
+    position = glm::vec3(0,0,0);
     Pitch = 0;
     Yaw = 0;
 }

head.h

@@ -42,6 +42,8 @@ class Head {
     
     float PupilConverge;
     
+    glm::vec3 position;
+    
 public:
     Head(void);
     void reset();
@@ -56,6 +58,8 @@ public:
     int transmit(char*);
     void receive(float);
     void SetNewHeadTarget(float, float);
+    glm::vec3 getPos() { return position; };
+    void setPos(glm::vec3 newpos) { position = newpos; };
 };
 
 #endif

interface.xcodeproj/project.pbxproj

@@ -108,17 +108,17 @@
 			isa = PBXGroup;
 			children = (
 				08FB7796FE84155DC02AAC07 /* main.cpp */,
-				B6BDAE4115F6BE4D002A07DF /* particle.h */,
-				B6BDAE4315F6BE53002A07DF /* particle.cpp */,
 				D4EE3BC015E746E900EE4C89 /* world.h */,
-				D4EE3BC415EBD90C00EE4C89 /* network.h */,
+				B6BDAE4315F6BE53002A07DF /* particle.cpp */,
-				D4EE3BC515EBD93400EE4C89 /* network.cpp */,
+				B6BDAE4115F6BE4D002A07DF /* particle.h */,
 				D4EE3BBD15E7465700EE4C89 /* field.cpp */,
+				D4EE3BBF15E7467600EE4C89 /* field.h */,
 				D4B96D4715FF966200CE6E8B /* head.h */,
 				D4B96D4815FF967C00CE6E8B /* head.cpp */,
 				D4EFE3CE162A2D7300DC5C59 /* hand.h */,
 				D4EFE3CF162A2DA000DC5C59 /* hand.cpp */,
-				D4EE3BBF15E7467600EE4C89 /* field.h */,
+				D4EE3BC515EBD93400EE4C89 /* network.cpp */,
+				D4EE3BC415EBD90C00EE4C89 /* network.h */,
 				D4EE3BBA15E45FFE00EE4C89 /* SerialInterface.h */,
 				D4EE3BBB15E45FFE00EE4C89 /* SerialInterface.cpp */,
 				D4EE3BC115E761B000EE4C89 /* util.cpp */,

interface.xcodeproj/xcuserdata/philip.xcuserdatad/xcdebugger/Breakpoints.xcbkptlist

@@ -8,13 +8,26 @@
          ignoreCount = "0"
          continueAfterRunningActions = "No"
          filePath = "main.cpp"
-         timestampString = "371979714.445172"
+         timestampString = "372278101.290877"
          startingColumnNumber = "9223372036854775807"
          endingColumnNumber = "9223372036854775807"
-         startingLineNumber = "318"
+         startingLineNumber = "328"
-         endingLineNumber = "318"
+         endingLineNumber = "328"
          landmarkName = "init(void)"
          landmarkType = "7">
       </FileBreakpoint>
+      <FileBreakpoint
+         shouldBeEnabled = "No"
+         ignoreCount = "0"
+         continueAfterRunningActions = "No"
+         filePath = "field.cpp"
+         timestampString = "372274896.176083"
+         startingColumnNumber = "9223372036854775807"
+         endingColumnNumber = "9223372036854775807"
+         startingLineNumber = "83"
+         endingLineNumber = "83"
+         landmarkName = "field_avg_neighbors(int index, glm::vec3 * result)"
+         landmarkType = "7">
+      </FileBreakpoint>
    </FileBreakpoints>
 </Bucket>

main.cpp

@@ -43,6 +43,7 @@
 #include "audio.h"
 #include "head.h"
 #include "hand.h"
+#include "particle.h"
 
 //TGAImg Img;
 
@@ -54,7 +55,7 @@ int serial_on = 0;                  //  Is serial connection on/off?  System wil
 //  Network Socket Stuff 
 //  For testing, add milliseconds of delay for received UDP packets
 int UDP_socket;
-int delay = 300;         
+int delay = 0;         
 char* incoming_packet;
 timeval ping_start;
 int ping_count = 0;
@@ -82,8 +83,21 @@ int HEIGHT = 800;
 #define BOTTOM_MARGIN 0				
 #define RIGHT_MARGIN 0
 
+#define HAND_RADIUS 0.25             //  Radius of in-world 'hand' of you
 Head myHead;                        //  The rendered head of oneself or others 
-Hand myHand;                        //  My hand (used to manipulate things in world)
+Hand myHand(HAND_RADIUS, 
+            glm::vec3(0,1,1));      //  My hand (used to manipulate things in world)
+
+glm::vec3 box(WORLD_SIZE,WORLD_SIZE,WORLD_SIZE);
+ParticleSystem balls(500, 
+                     box, 
+                     false,                     // Wrap?
+                     0.0,                       // Noise
+                     0.3,                        //  Size scale 
+                     0.0                       // Gravity 
+                     );
+
+
 
 //  FIELD INFORMATION 
 //  If the simulation 'world' is a box with 10M boundaries, the offset to a field cell is given by:
@@ -97,7 +111,8 @@ Hand myHand;                        //  My hand (used to manipulate things in wo
 
 #define RENDER_FRAME_MSECS 10
 #define SLEEP 0
-#define NUM_TRIS 20000   //000
+
+#define NUM_TRIS 100  // 20000   //000
 struct {
     float vertices[NUM_TRIS * 9];
     float normals [NUM_TRIS * 3];
@@ -176,11 +191,6 @@ timeval last_frame;
 double elapsedTime;
 
 
-float randFloat () {
-    return (rand()%10000)/10000.f;
-}
-
-
 //  Every second, check the frame rates and other stuff 
 void Timer(int extra)
 {
@@ -354,7 +364,6 @@ const float SCALE_Y = 1.f;
 void update_tris()
 {
     int i;
-    float dist_sqrd;
     float field_val[3];
     
     for (i = 0; i < NUM_TRIS; i++)
@@ -374,32 +383,15 @@ void update_tris()
             tris.vertices[i*9+5] += tris.vel[i*3+2];
             tris.vertices[i*9+8] += tris.vel[i*3+2]; 
             
-            if (0)
-            {
-                dist_sqrd = tris.vertices[i*9+0]*tris.vertices[i*9+0] +
-                            tris.vertices[i*9+1]*tris.vertices[i*9+1] + 
-                            tris.vertices[i*9+2]*tris.vertices[i*9+2];
-                
-                if (dist_sqrd > 1.0)
-                {
-                    glm::vec3 pos (tris.vertices[i*9+0],tris.vertices[i*9+1], tris.vertices[i*9+2]);
-                    glm::normalize(pos);
-                    pos*=-1/dist_sqrd*0.0001;
-                
-                    tris.vel[i*3] += pos.x;
-                    tris.vel[i*3+1] += pos.y;
-                    tris.vel[i*3+2] += pos.z;
-                }
-            }
             
             // Add a little gravity 
-            const float GRAVITY = 0.0001;
+            //tris.vel[i*3+1] -= 0.0001;
-            tris.vel[i*3+1] -= GRAVITY;
            
+            const float DRAG = 0.99;
             // Drag:  Decay velocity
-            tris.vel[i*3] *= 0.99;
+            tris.vel[i*3] *= DRAG;
-            tris.vel[i*3+1] *= 0.99;
+            tris.vel[i*3+1] *= DRAG;
-            tris.vel[i*3+2] *= 0.99;
+            tris.vel[i*3+2] *= DRAG;
         }
                  
         if (tris.element[i] == 1) 
@@ -418,12 +410,7 @@ void update_tris()
         // Y-direction
         if ((tris.vertices[i*9+1] > WORLD_SIZE) || (tris.vertices[i*9+1] < 0.0))
         { 
-            //tris.vel[i*3+1]*= -1.0;
+            tris.vel[i*3+1]*= -1.0;
-            if (tris.vertices[i*9+1] < 0.0)
-            {
-                tris.vertices[i*9+1] = tris.vertices[i*9+4] = tris.vertices[i*9+7] = WORLD_SIZE;
-                //tris.vel[i*3+1]*= -1.0;
-            }
         }
         // Z-Direction
         if ((tris.vertices[i*9+2] > WORLD_SIZE) || (tris.vertices[i*9+2] < 0.0))
@@ -563,6 +550,10 @@ void update_pos(float frametime)
     //  Slide location sideways
     location[0] += fwd_vec[2]*-lateral_vel;
     location[2] += fwd_vec[0]*lateral_vel;
+    
+    //  Update head and manipulator objects with object with current location
+    myHead.setPos(glm::vec3(location[0], location[1], location[2]));
+    balls.updateHand(myHead.getPos() + myHand.getPos(), glm::vec3(0,0,0), myHand.getRadius());
 }
 
 void display(void)
@@ -597,21 +588,15 @@ void display(void)
         glRotatef(render_yaw, 0, 1, 0);
         glTranslatef(location[0], location[1], location[2]);
 
-        glEnable(GL_DEPTH_TEST);
+        glEnable(GL_DEPTH_TEST);        
-        
+        //  TEST:  Draw a reference object in world space coordinates! 
-        //  Draw a few 'planets' to find and explore 
         glPushMatrix();
-            glTranslatef(1.f, 1.f, 1.f);
+            glTranslatef(1,0,0);
-            glColor3f(1, 0, 0); 
+            //glTranslatef(myHead.getPos().x, myHead.getPos().y, myHead.getPos().z);
-            glutSolidSphere(0.6336, 20, 20); 
+            glColor3f(1,0,0);
-            glTranslatef(5, 5, 5);
+            glutSolidCube(0.4); 
-            glColor3f(1, 1, 0); 
-            glutSolidSphere(0.4, 20, 20); 
-            glTranslatef(-2.5, -2.5, 2.5);
-            glColor3f(1, 0, 1); 
-            glutSolidSphere(0.3, 20, 20); 
         glPopMatrix();
-        
+    
         // Draw Triangles 
         
         glBegin(GL_TRIANGLES);
@@ -643,9 +628,11 @@ void display(void)
             myHead.render();
         }
         myHand.render();
-        
+    
-    //  Render the world box 
+        balls.render();
-    render_world_box();
+            
+        //  Render the world box 
+        render_world_box();
 
     glPopMatrix();
 
@@ -657,6 +644,8 @@ void display(void)
         glDisable(GL_DEPTH_TEST);
         glDisable(GL_LIGHTING);
 
+        drawvec3(100, 100, 0.15, 0, 1.0, 0, myHead.getPos(), 0, 1, 0);
+    
         if (mouse_pressed == 1)
         {
             glPointSize(20.f);
@@ -799,8 +788,10 @@ void idle(void)
         //  Simulation
         update_pos(1.f/FPS); 
         update_tris();
+        field_simulate(1.f/FPS);
         myHead.simulate(1.f/FPS);
         myHand.simulate(1.f/FPS);
+        balls.simulate(1.f/FPS);
 
         if (!step_on) glutPostRedisplay();
         last_frame = check;

particle.cpp

@@ -8,51 +8,203 @@
 
 #include "particle.h"
 
+#define NUM_ELEMENTS 4
+
+glm::vec3 color0(1,0,0);             //  Motionless particle
+glm::vec3 color1(0,1,0);             //  Spring force 
+glm::vec3 color2(0,0,1);
+glm::vec3 color3(0,1,1);
+
+float radii[NUM_ELEMENTS] = {0.3, 0.5, 0.2, 0.4};
+
+ParticleSystem::ParticleSystem(int num, 
+                               glm::vec3 box, 
+                               int wrap, 
+                               float noiselevel, 
+                               float setscale,
+                               float setgravity) {
+    //  Create and initialize particles 
+    int i, element;
+    bounds = box;
+    count = num;
+    wrapBounds = wrap;
+    noise = noiselevel;
+    gravity = setgravity;
+    scale = setscale; 
+    particles = new Particle[count];
+    
+    for (i = 0; i < count; i++) {
+        particles[i].position.x = randFloat()*box.x;
+        particles[i].position.y = randFloat()*box.y;
+        particles[i].position.z = randFloat()*box.z;
+        
+        particles[i].velocity.x = 0;
+        particles[i].velocity.y = 0;
+        particles[i].velocity.z = 0;
+        
+        particles[i].parent = 0; 
+        particles[i].link *= 0;
+        
+        element = rand()%NUM_ELEMENTS;
+        particles[i].element = element; 
+        
+        if (element == 0) particles[i].color = color0;
+        else if (element == 1) particles[i].color = color1;
+        else if (element == 2) particles[i].color = color2;
+        else if (element == 3) particles[i].color = color3;
+        
+        particles[i].radius = radii[element]*scale;
+        particles[i].isColliding = false;
+        
+        
+    }
+}
+
+bool ParticleSystem::updateHand(glm::vec3 pos, glm::vec3 vel, float radius) {
+    handPos = pos;
+    handVel = vel;
+    handRadius = radius;
+    return handIsColliding;
+}
+
+void ParticleSystem::resetHand() {
+    handActive = false; 
+    handIsColliding = false;
+    handPos = glm::vec3(0,0,0);
+    handVel = glm::vec3(0,0,0);
+    handRadius = 0;
+}
+
+void ParticleSystem::render() {
+    for (unsigned int i = 0; i < count; ++i) {
+        glPushMatrix();
+            glTranslatef(particles[i].position.x, particles[i].position.y, particles[i].position.z);
+            if (particles[i].isColliding) glColor3f(particles[i].color.x * 0.7, 
+                                                    particles[i].color.y * 0.7, 
+                                                    particles[i].color.z * 0.7);
+            else glColor3f(particles[i].color.x, particles[i].color.y, particles[i].color.z);
+            glutSolidSphere(particles[i].radius, 15, 15);
+        glPopMatrix();
+    }
+}
+
+void ParticleSystem::link(int child, int parent) {
+    particles[child].parent = parent; 
+    particles[child].velocity *= 0.5;
+    particles[parent].velocity += particles[child].velocity;
+    particles[child].velocity *= 0.0;
+    particles[child].color = glm::vec3(1,1,0);
+    particles[child].link = particles[parent].position - particles[child].position;
+}
+
 void ParticleSystem::simulate (float deltaTime) {
-    for (unsigned int i = 0; i < particleCount; ++i) {
+    int i, j;
-        
+    for (i = 0; i < count; ++i) {
-        // Move particles
+        if (particles[i].element != 0) {
-        particles[i].position += particles[i].velocity * deltaTime;
+            
-        
+            if (particles[i].parent == 0) {
-        // Add gravity
+                // Move particles
-        particles[i].velocity.y -= GRAVITY;
+                particles[i].position += particles[i].velocity * deltaTime;
-        
-        // Drag: decay velocity
-        particles[i].velocity *= 0.99;
-        
-        // Add velocity from field
-        //Field::addTo(particles[i].velocity);
-        //particles[i].velocity += Field::valueAt(particles[i].position);
-   
-        if (wrapBounds) {
-            // wrap around bounds
-            if (particles[i].position.x > bounds.x)
-                particles[i].position.x -= bounds.x;
-            else if (particles[i].position.x < 0.0f)
-                particles[i].position.x += bounds.x;
                 
-            if (particles[i].position.y > bounds.y)
+                // Add gravity
-                particles[i].position.y -= bounds.y;
+                particles[i].velocity.y -= gravity*deltaTime;
-            else if (particles[i].position.y < 0.0f)
-                particles[i].position.y += bounds.y;
                 
-            if (particles[i].position.z > bounds.z)
+                // Drag: decay velocity
-                particles[i].position.z -= bounds.z;
+                particles[i].velocity *= 0.99;
-            else if (particles[i].position.z < 0.0f)
+               
-                particles[i].position.z += bounds.z;
+                // Add velocity from field
-        } else {
+                //Field::addTo(particles[i].velocity);
-            // Bounce at bounds
+                //particles[i].velocity += Field::valueAt(particles[i].position);
-            if (particles[i].position.x > bounds.x 
+           
-            || particles[i].position.x < 0.f) {
+                // Add noise 
-                particles[i].velocity.x *= -1;
+                const float RAND_VEL = 3.0;
+                if (noise) {
+                    if (randFloat() < noise*deltaTime) {
+                        particles[i].velocity += glm::vec3((randFloat() - 0.5)*RAND_VEL,
+                                                           (randFloat() - 0.5)*RAND_VEL,
+                                                           (randFloat() - 0.5)*RAND_VEL);
+                        }
+                    } 
+            } else {
+                particles[i].position = particles[particles[i].parent].position + particles[i].link;
             }
-            if (particles[i].position.y > bounds.y 
+            
-            || particles[i].position.y < 0.f) {
+            
-                particles[i].velocity.y *= -1;
+            //  Check for collision with manipulator hand 
+            particles[i].isColliding = (glm::vec3(particles[i].position - handPos).length() < 
+                                        (radius + handRadius));
+            
+            //  Check for collision with other balls
+            float separation;
+            const float HARD_SPHERE_FORCE = 100.0;
+            const float SPRING_FORCE = 0.1;
+            float spring_length = 3*radii[1];
+            float contact; 
+            
+            particles[i].isColliding = false; 
+            
+            for (j = 0; j < count; j++) {
+                if ((j != i) && 
+                    (!particles[i].parent)) {
+                    separation = glm::distance(particles[i].position, particles[j].position);
+                    contact = particles[i].radius + particles[j].radius;
+                    
+                    //  Hard Sphere Scattering
+                    if (separation < contact) {
+                        particles[i].velocity += glm::normalize(particles[i].position - particles[j].position)*deltaTime*HARD_SPHERE_FORCE*(contact - separation);
+                        particles[i].isColliding = true;
+                    } 
+                    
+                    //  Spring Action
+                    if ((particles[i].element == 1) && (separation < spring_length*2)) {
+                        particles[i].velocity += glm::normalize(particles[i].position - particles[j].position)*deltaTime*SPRING_FORCE*(spring_length - separation);
+                    }
+                     
+                    //  Link!
+                    if ((particles[i].parent == 0) && 
+                        (particles[j].parent != i) &&
+                        (separation > 0.9*(particles[j].radius + particles[i].radius)) &&
+                        (separation < 1.0*(particles[j].radius + particles[i].radius)) ) {
+                            //  Link i to j!!
+                            //link(i, j);
+                        }
+                    
+                        
+                }
             }
-            if (particles[i].position.z > bounds.z 
+            
-            || particles[i].position.z < 0.f) {
+            if (!particles[i].parent) {
-                particles[i].velocity.z *= -1;
+                if (wrapBounds) {
+                    // wrap around bounds
+                    if (particles[i].position.x > bounds.x)
+                        particles[i].position.x -= bounds.x;
+                    else if (particles[i].position.x < 0.0f)
+                        particles[i].position.x += bounds.x;
+                        
+                    if (particles[i].position.y > bounds.y)
+                        particles[i].position.y -= bounds.y;
+                    else if (particles[i].position.y < 0.0f)
+                        particles[i].position.y += bounds.y;
+                        
+                    if (particles[i].position.z > bounds.z)
+                        particles[i].position.z -= bounds.z;
+                    else if (particles[i].position.z < 0.0f)
+                        particles[i].position.z += bounds.z;
+                } else {
+                    // Bounce at bounds
+                    if (particles[i].position.x > bounds.x 
+                    || particles[i].position.x < 0.f) {
+                        particles[i].velocity.x *= -1;
+                    }
+                    if (particles[i].position.y > bounds.y 
+                    || particles[i].position.y < 0.f) {
+                        particles[i].velocity.y *= -1;
+                    }
+                    if (particles[i].position.z > bounds.z 
+                    || particles[i].position.z < 0.f) {
+                        particles[i].velocity.z *= -1;
+                    }
+                }
             }
         }
     }

particle.h

@@ -10,22 +10,52 @@
 #define interface_particle_h
 
 #include "glm/glm.hpp"
-
+#include "util.h"
-#define GRAVITY 0.0001
+#include "world.h"
+#include <GLUT/glut.h>
 
 class ParticleSystem {
 public:
-    void simulate (float deltaTime);
+    ParticleSystem(int num, 
-    void draw ();
+                   glm::vec3 box, 
+                   int wrap, 
+                   float noiselevel,
+                   float setscale,
+                   float setgravity);
+    
+    void simulate(float deltaTime);
+    void render();
+    bool updateHand(glm::vec3 pos, glm::vec3 vel, float radius);
 
 private:
     struct Particle {
-        glm::vec3 position, velocity;
+        glm::vec3 position, velocity, color, link;
+        int element;
+        int parent;
+        float radius;
+        bool isColliding;
     } *particles;
-    unsigned int particleCount;
+    unsigned int count;
     
     glm::vec3 bounds;
-    const static bool wrapBounds = false;
+    
+    float radius;
+    bool wrapBounds;
+    float noise;
+    float gravity;
+    float scale; 
+    glm::vec3 color;
+    
+    void link(int child, int parent);
+    
+    //  Manipulator from outside
+    void resetHand();
+    bool handActive; 
+    bool handIsColliding;
+    glm::vec3 handPos;
+    glm::vec3 handVel;
+    float handRadius;
+    
 };
 
 #endif

util.cpp

@@ -13,6 +13,12 @@
 #endif
 #include <iostream>
 #include "world.h"
+#include "glm/glm.hpp"
+
+
+float randFloat () {
+    return (rand()%10000)/10000.f;
+}
 
 void render_world_box()
 {
@@ -20,28 +26,16 @@ void render_world_box()
     glDisable(GL_LIGHTING);
     glColor4f(1.0, 1.0, 1.0, 1.0);
     glLineWidth(1.0);
-    glBegin(GL_LINE_STRIP);
-    glVertex3f(0,0,0);
-    glVertex3f(WORLD_SIZE,0,0);
-    glVertex3f(WORLD_SIZE,WORLD_SIZE,0);
-    glVertex3f(0,WORLD_SIZE,0);
-    glVertex3f(0,0,0);
-    glVertex3f(0,0,WORLD_SIZE);
-    glVertex3f(WORLD_SIZE,0,WORLD_SIZE);
-    glVertex3f(WORLD_SIZE,WORLD_SIZE,WORLD_SIZE);
-    glVertex3f(0,WORLD_SIZE,WORLD_SIZE);
-    glVertex3f(0,0,WORLD_SIZE);
-    glEnd();
-
     glBegin(GL_LINES);
-    glVertex3f(0,WORLD_SIZE,0);
+    glColor3f(1,0,0);
-    glVertex3f(0,WORLD_SIZE,WORLD_SIZE);
+    glVertex3f(0,0,0);
-
-    glVertex3f(WORLD_SIZE,WORLD_SIZE,0);
-    glVertex3f(WORLD_SIZE,WORLD_SIZE,WORLD_SIZE);
-
     glVertex3f(WORLD_SIZE,0,0);
-    glVertex3f(WORLD_SIZE,0,WORLD_SIZE);
+    glColor3f(0,1,0);
+    glVertex3f(0,0,0);
+    glVertex3f(0, WORLD_SIZE, 0);
+    glColor3f(0,0,1);
+    glVertex3f(0,0,0);
+    glVertex3f(0, 0, WORLD_SIZE);
     glEnd();
 }
 
@@ -77,3 +71,30 @@ void drawtext(int x, int y, float scale, float rotate, float thick, int mono, ch
     glPopMatrix();
 
 }
+
+
+void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, glm::vec3 vec, 
+              float r=1.0, float g=1.0, float b=1.0)
+{
+    //
+    //  Draws text on screen as stroked so it can be resized
+    //
+    char vectext[20];
+    sprintf(vectext,"%3.1f,%3.1f,%3.1f", vec.x, vec.y, vec.z);
+    int len, i;
+    glPushMatrix();
+    glTranslatef(x, y, 0);
+    glColor3f(r,g,b);
+    glRotated(180+rotate,0,0,1);
+    glRotated(180,0,1,0);
+    glLineWidth(thick);
+    glScalef(scale, scale, 1.0);
+    len = (int) strlen(vectext);
+	for (i = 0; i < len; i++)
+	{
+        if (!mono) glutStrokeCharacter(GLUT_STROKE_ROMAN, int(vectext[i]));
+        else glutStrokeCharacter(GLUT_STROKE_MONO_ROMAN, int(vectext[i]));
+	}
+    glPopMatrix();
+    
+} 

util.h

@@ -8,10 +8,14 @@
 
 #ifndef interface_util_h
 #define interface_util_h
+#include "glm/glm.hpp"
 
+float randFloat();
 void render_world_box();
 void drawtext(int x, int y, float scale, float rotate, float thick, int mono, char *string, 
               float r=1.0, float g=1.0, float b=1.0);
+void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, glm::vec3 vec, 
+              float r=1.0, float g=1.0, float b=1.0);
 double diffclock(timeval clock1,timeval clock2);
 
 #endif

world.h

@@ -15,6 +15,4 @@ const float WORLD_SIZE = 10.0;
 #define PI 3.14159265
 
 
-float randFloat();
-
 #endif