VisClient/org/hfbk/vis/visnode/VisUeberwach.java

Go to the documentation of this file.

package org.hfbk.vis.visnode;

import java.util.ArrayList;
import java.util.Date;
import java.util.HashMap;
import java.util.List;

import org.dronus.gl.GLBoxRenderer;
import org.dronus.gl.GLSphereRenderer;
import org.dronus.gl.GLFont;
import org.dronus.gl.GLUtil;
import org.dronus.graph.Node;
import org.hfbk.vis.Prefs;
import org.lwjgl.opengl.GL11;
import org.lwjgl.util.vector.Vector3f;

public class VisUeberwach extends VisNode {

        final float DVARIANCE_DT=10f; // units/s
        final float NEW_COST=5f;
        final float EXIT_VARIANCE=100;
        

        
        class Item{
                Vector3f pos;
                Vector3f dpos=new Vector3f();
        }
        
        class VisTrack extends VisNode{
                
                int id;
                
                final int PAST_STATES=10;
                int index; //index to this items ring buffer
                
                Item[] past=new Item[PAST_STATES];
                
                Item current;
                
                float variance;
                
                
                public VisTrack() {
                        super(null, new Vector3f());
                        id=maxTrackId++;
                        Item tp=new Item();
                        current=past[0]=tp;     
                        radius=10000;
                }
                
                int push(){
                        Item last=getOld(0);
                        Item tp=new Item();
                        tp.pos=new Vector3f(last.pos); tp.dpos=new Vector3f(last.dpos);
                        
                        index++;
                        index%=PAST_STATES;
                        past[index]=tp;
                        current=tp;
                        
                        return index;                   
                }
                
                protected void renderSelf() {
                        float dt=getRoot().client.dt;                   
                                
                        Item tp=past[index];
                        Vector3f d=new Vector3f(tp.dpos);
                        d.scale(dt);
                        Vector3f.add(tp.pos, d, tp.pos);
                        variance+=DVARIANCE_DT*dt;
                        
                        position.set(current.pos);
                        
                        
                        float c=id*523.724325f;  //calculate some unique color by the id
                        GL11.glColor4f(1-c%1,(c*3)%1,c%1,1/(1+variance/100)); //alpha is controlled by variance   
                        GLSphereRenderer.renderSphere(variance/5);
                        
                        
                        GL11.glTranslatef(-4,2,0);
                        GLUtil.billboardCylinder();
                        GL11.glColor3f(1,1,1);
                        GLFont.getDefault().render();
                        GLFont.getDefault().print(getDatasheet());
                        GL11.glDisable(GL11.GL_TEXTURE_2D);
                        
                        
                        
                        
                        if(variance>EXIT_VARIANCE)
                                parent.remove(this);
                }
                
                Item getOld(int lookback){
                        int i=(index-lookback)%10;
                        return past[i];
                }

                long created=System.currentTimeMillis();
                long lastseen=created;
                String getDatasheet(){
                        return  
                        "Known since: "+(new Date(created))+"\n"+
                        "Last spotted: "+(new Date(lastseen))+"\n"+
                        "ID:"+hashCode()+"\n";
                }
                
                protected void transform() {
                        super.transform();
                        GL11.glRotatef(90, 0, 1, 0);
                }
        }
        
        interface Sensor{
                abstract float    getCost(VisTrack track);
                abstract void update(VisTrack track);
        }
        
        
        int maxTrackId=0;
        
        
        public VisUeberwach(Node node, Vector3f position) {
                super(node, position);
                radius=Float.MAX_VALUE;
        }
        
        static Vector3f lerp(Vector3f a, Vector3f b, float t){
                a=new Vector3f(a); b=new Vector3f(b);
                a.scale(1-t); b.scale(t);
                Vector3f result=new Vector3f();
                Vector3f.add(a,b, result);
                return result;
        } 
        
        void trackOne(Sensor s){
                List<VisTrack> tracks=new ArrayList<VisTrack>();
                
                for (VisNode n: children)
                        if (n instanceof VisTrack)
                                tracks.add((VisTrack)n);
                
                int nt=tracks.size();
                
                float[] costs=new float[nt+1];  //one more item for possibly new one
                for (int t=0; t<nt; t++)
                        costs[t]=s.getCost(tracks.get(t))/tracks.get(t).variance;
                
                costs[nt]=NEW_COST;     //possible new item propability
                
                //find most likely Track to acommodate this sensors data
                // (nearest neighbour)
                int minT=-1;
                float minCost=Float.MAX_VALUE;
                for (int t=0; t<=nt; t++)
                        if (costs[t]<minCost){
                                minCost=costs[t];
                                minT=t;
                        }
                
                VisTrack t;
                if (minT==nt){  //most likely we got a new thing
                        t=new VisTrack();
                        add(t);
                }       
                else t=tracks.get(minT);
                
                s.update(t); //update Track with Sensors data.
        }

        
        class VisCamera extends VisNode {
                float heading, pitch; 
                
                public VisCamera(Vector3f pos, float heading, float pitch,  String ip) {
                        super(null, pos);
                        this.heading=heading;
                        this.pitch=pitch;
                        cameras.put(ip,this);
                }
                
                protected void transform() {
                        super.transform();
                        GL11.glRotatef(heading, 0,1,0);
                        GL11.glRotatef(pitch,   1,0,0);                 
                }
                
                protected void renderSelf() {
                        GL11.glDisable(GL11.GL_TEXTURE_2D);
                        GL11.glColor4f(.3f,.5f,1,.5f);
                        GLBoxRenderer.renderBox(3,3,1);
                        GL11.glBegin(GL11.GL_LINES);
                        GL11.glVertex3f(0,0,0); GL11.glVertex3f(-7,-5,50);
                        GL11.glVertex3f(0,0,0); GL11.glVertex3f( 7,-5,50);
                        GL11.glVertex3f(0,0,0); GL11.glVertex3f( 7, 5,50);
                        GL11.glVertex3f(0,0,0); GL11.glVertex3f(-7, 5,50);
                        GL11.glEnd();
                }
        }
        
        class VisSonar extends VisNode implements Sensor{
                
                float distance, speed;
                
                float maxdistance; //for static filtering (walls etc.)
                
                float timer;
                final float TIMEOUT=1; //seconds
                final float MAX_FUZZ=5; //units
                final float WEIGHT_DIST=1, WEIGHT_SPEED=0;
                
                float heading, pitch; 
                
                public VisSonar(Vector3f pos, float heading, float pitch) {
                        super(null, pos);
                        this.heading=heading;
                        this.pitch=pitch;
                        radius=10000;
                }
                
                void receive(float distance, float speed){
                        //only accept non-static values
                        maxdistance=Math.max(maxdistance, distance);
                        if (distance>maxdistance-MAX_FUZZ) return;                              
                        
                        this.distance=distance; this.speed=speed;                                               
                        trackOne(this);
                        timer=0;
                }
                
                protected void transform() {
                        super.transform();
                        GL11.glRotatef(heading, 0,1,0);
                        GL11.glRotatef(pitch,   1,0,0);                 
                }
                
                void renderSelf() {
                        
                        timer+=getRoot().client.dt;
                        if (timer>TIMEOUT){
                                distance=speed=0;
                        }
                        
                        GL11.glDisable(GL11.GL_TEXTURE_2D);
                        GL11.glColor4f(.3f,.5f,1,.5f);
                        GL11.glPushMatrix();
                        GL11.glScalef(1,1,.1f);
                        GLSphereRenderer.renderSphere(2);
                        GL11.glPopMatrix();
                        GL11.glTranslatef(0,0,5);
                        GL11.glColor3f(.3f,.5f,1);
                        GLBoxRenderer.renderBox(.2f,.2f,10f);
                        GL11.glTranslatef(0,0,-5);
                        
                        if (distance!=0){ //show some echo
                                GL11.glColor3f(1,1,1);
                                GL11.glTranslatef(0,0,distance);
                                GLSphereRenderer.renderSphere(1);                                                               
                        }                       
                }

                public float getCost(VisTrack track) {
                        if (distance==0) return Float.MAX_VALUE; 
                        
                        Vector3f pos=parent.traverse(this,track.current.pos); //get items current position   
                        
                        Vector3f tmp=new Vector3f();                                            //calculate items next position 
                        Vector3f.add(track.current.pos, track.current.dpos, tmp);
                        Vector3f pos2=parent.traverse(this, tmp);
                        
                        float dl=pos2.length()-pos.length();   // calculate velocity of distance change 
//                      System.out.println(dl+" "+speed);
                        
                        float cost= Math.abs(pos.length()-distance)*WEIGHT_DIST 
                                  + Math.abs(dl-speed)             *WEIGHT_SPEED;
                        
                        return cost;
                }

                public void update(VisTrack track) {
                        
                        Vector3f pos=traverse(parent, new Vector3f(0,0,distance));
                        
                        //System.out.println(track.current.pos+" "+pos);
                        
                        if (track.current.pos==null) track.current.pos=pos;
                        else                         track.current.pos.set(pos); //lerp(track.current.pos, pos, .5f);
                        
                        Vector3f pos2=traverse(parent, new Vector3f(0,0,distance+speed));
                        Vector3f.sub(pos2, pos, track.current.dpos);
                        
                        track.variance=1f;
                        track.lastseen=System.currentTimeMillis();
                }               
        }
        
        
        public VisNode addSonar(Vector3f pos, float heading, float elevation){
                VisNode sonar= new VisSonar(pos,heading,elevation);
                add(sonar);
                return sonar;
        }
        
        public VisNode addCamera(Vector3f pos, float heading, float elevation, String ip){
                VisNode cam= new VisCamera(pos,heading,elevation,ip);
                add(cam);
                return cam;
        }
        
        
        class VisSonarDummy extends VisSonar{

                public VisSonarDummy(Vector3f pos, float heading, float pitch) {
                        super(pos, heading, pitch);
                }
                
                float olddist=10; //hold last simulated items position
                
                int dir=1;
                
                void renderSelf() {
                        
                        if (Math.random()>.99f){ //ocassionally invoke a simulated receive! 
                                
                                /*if (distance>50 || distance < 0) dir=-dir;
                                
                                float d= dir * (float)Math.random();                            
                                receive(olddist+d, dir*0.00002f/getRoot().client.dt);                           
                                olddist=distance;
*/
                                
                                if (olddist>100 || olddist <= 5) dir=-dir;
                                
                                float d= dir * ((float)Math.random()*.2f+1);                            
                                receive(olddist + 100 * d * getRoot().client.dt, d);                            
                                olddist=distance;

                                trackOne(this); //usually invoked by top class!!
                        }
                        super.renderSelf();
                }               
        }
        
        final float SCALE=.1f, DSCALE=.01f; //scale for distance and speed
        // m,  m/s

        
        public void parseMsg(String[] msg){
                if (!msg[1].matches("sonar[0-9]+")) return;
                int id=Integer.parseInt(msg[1].replace("sonar", ""));
                if (id>0 && id<=children.size()){
                        int distance=Integer.decode(msg[2]),speed=Integer.decode(msg[3]);
                        
                        if (speed>0x7FFF) speed-=0xFFFF;
                        if (Math.abs(speed)>30) speed=0; 
                        
                        ((VisSonar)children.get(id-1)).receive(distance*SCALE, speed*DSCALE);
                        
                        //System.out.println("Echo @ "+id+": "+distance+", "+speed);
                }
        }
        
        void renderSelf() {             

        }

        
        
        HashMap<String, VisCamera> cameras=new HashMap<String, VisCamera>();
        public void addPortrait(String path, String ip) {
                
                if(Prefs.current.verbose) System.out.println("Cam "+ip+": "+path);
                
                Vector3f camera=cameras.get(ip).position;
                if (camera==null) return;
                if(Prefs.current.verbose) System.out.println("known. ");
                
                float minDist=Float.MAX_VALUE;
                VisNode minTrack=null;
                for (VisNode n: children)
                        if(n instanceof VisTrack){
                                float dist=Vector3f.sub(n.position, camera,null).lengthSquared();
                                if (dist<minDist){
                                        minDist=dist;
                                        minTrack=n;
                                }
                        }
                if (minTrack!=null)  minTrack.add(new VisImage(new Node(path), new Vector3f()));
        }

}

---