world.java

一款机器人仿真软件,功能上与microsoft robotics studio有些相似,但基于Java平台.突出特点是给出了传感器仿真,如声纳,激光等.

/*
 *  Simbad - Robot Simulator
 *  Copyright (C) 2004 Louis Hugues
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful, 
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 -----------------------------------------------------------------------------
 * $Author: sioulseuguh $ 
 * $Date: 2005/08/07 12:25:03 $
 * $Revision: 1.16 $
 * $Source: /cvsroot/simbad/src/simbad/sim/World.java,v $
 */
package simbad.sim;

import java.awt.GraphicsConfigTemplate;
import java.awt.GraphicsConfiguration;
import java.awt.GraphicsEnvironment;
import java.util.Map;
import javax.media.j3d.*;

import javax.vecmath.*;
import com.sun.j3d.utils.geometry.Primitive;
import com.sun.j3d.utils.geometry.Sphere;



/**
 * Represents a 3d world - this class use intensively JAVA3D.
 * It creates the scenegraph and the view platform.
 * The building process uses a EnvironmentDescription object containing  colors ,
 *  objects and attributes given by the user.  This class is thighly coupled with Simulator Class.
 * 
 * Remember to call System.setProperty("j3d.implicitAntialiasing", "true") at the 
 * very beginning of your program in order to enable antialiasing. 
 *
 */
public  class World   {

    /** J3D universe */
    VirtualUniverse universe;
    /** The content branch. */
    private BranchGroup sceneBranch;
    /** The base translation of the sceneBranch. */
    private TransformGroup sceneTrans;
    /** The base rotation of the sceneBranch. */
    private TransformGroup sceneRot;

 
    /** All the pickable/collidable objects should be attached under this sub-branch */
    private BranchGroup  pickableSceneBranch;
    /** The view branch */ 
    private BranchGroup viewBranch;
    private TransformGroup viewTransformGroup;
    private ViewPlatform viewPlatform;
    private View view;
    
        
    /** The main canvas for viewing the world. */   
    private Canvas3D canvas3d;
    /** For managing the mouse mouvement in the Canvas3D */
    MouseOrbiter mouseOrbiter;

    // lights
 
    private Light light1;
    private Light light2;
    
     
    // constants used to specify the viewpoint
    public static final int VIEW_FROM_TOP=1;
    public static final int VIEW_FROM_EAST=2;
    public static final int VIEW_BEHIND_AGENT=3;
    public static final int VIEW_ABOVE_AGENT=4;
    public static final int VIEW_ABOVE_AGENT_NEAR=5;
    public static final int VIEW_AGENT_SIDE=6;
          

    /** The size of the square containing the world */
    protected  float worldSize;

    /** Prepared colors. */
    Color3f white = new Color3f(1,1,1);
    Color3f black = new Color3f(0,0,0);

    
    /** Construct a World from a given  EnvironmentDescription. */
    public World(EnvironmentDescription ed) {
        create(ed);
    }

    /** Creates the world from the given environement Description. 
     * Used only in the creation phase.
     * @param ed the environment description.
     */
    private void create(EnvironmentDescription ed) {
        worldSize = ed.worldSize;
        createUniverse(ed);
   
    }
    
   
    /** Creates the universe to attach the scenegraph. 
     * Used only in the creation phase.
     * @param ed the environment description.
     */
    private void createUniverse(EnvironmentDescription ed) {
        System.out.println("create Universe");
        // show infos 
        Map map  = VirtualUniverse.getProperties();
        System.out.println("----------------------------------------");
        System.out.println("j3d.version = "+map.get("j3d.version"));
        System.out.println("j3d.vendor = "+map.get("j3d.vendor"));
        System.out.println("j3d.specification.version = "+map.get("j3d.specification.version"));
        System.out.println("j3d.specification.vendor = "+map.get("j3d.specification.vendor"));
        System.out.println("j3d.renderer = "+map.get("j3d.renderer"));
        System.out.println("J3DThreadPriority = "+VirtualUniverse.getJ3DThreadPriority());
        System.out.println("----------------------------------------");

         createCanvas3D();
        createSceneBranch(ed);

        universe = new VirtualUniverse();
           
        Locale locale = new Locale(universe);
 
        // Create and add VIEW branch
        // locale->viewBranch->viewTransformGroup->viewPlatform
        viewBranch = new BranchGroup();
        viewTransformGroup = new TransformGroup();
        viewTransformGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        viewTransformGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        Transform3D t3d =new Transform3D();
        t3d.setIdentity();
        viewTransformGroup.setTransform(t3d);
        viewBranch.addChild(viewTransformGroup);
        
        // Creates View and viewplatform
        viewPlatform = new ViewPlatform();
        viewPlatform.setViewAttachPolicy(View.NOMINAL_HEAD);
        viewPlatform.setActivationRadius(100);
        view = new View();
        view.setProjectionPolicy(View.PERSPECTIVE_PROJECTION);
        
          view.setViewPolicy(View.SCREEN_VIEW);
        view.setVisibilityPolicy(View.VISIBILITY_DRAW_ALL);
        
        view.setFrontClipDistance(0.02);
        
        GraphicsConfigTemplate3D template = new GraphicsConfigTemplate3D();
        template.setSceneAntialiasing(GraphicsConfigTemplate.REQUIRED);
        template.setDoubleBuffer(GraphicsConfigTemplate.PREFERRED);
       /* GraphicsConfiguration config = GraphicsEnvironment
                .getLocalGraphicsEnvironment().getDefaultScreenDevice()
                .getBestConfiguration(template);*/
        // request antialiasing
        view.setSceneAntialiasingEnable(true);
        
 
        view.addCanvas3D(canvas3d);
        PhysicalBody phyBody = new PhysicalBody();
        PhysicalEnvironment phyEnv = new PhysicalEnvironment();
        view.setPhysicalBody(phyBody);
        view.setPhysicalEnvironment(phyEnv);
        view.attachViewPlatform(viewPlatform);
        viewTransformGroup.addChild(viewPlatform);
        
        // Add both branch to the unique locale
        locale.addBranchGraph(viewBranch);
        locale.addBranchGraph(sceneBranch);
      
        
        // Add mouse control in the canvas3d 
        mouseOrbiter = new MouseOrbiter(canvas3d,viewTransformGroup);

        	// sets initial viewpoint
        changeViewPoint(ed.worldViewPoint,null);
    }
    
    /** Creates the Canvas3D to visualize the 3D World. 
     * Used only in the creation phase.
     */
    private void createCanvas3D(){
         GraphicsConfigTemplate3D template = new GraphicsConfigTemplate3D();
        template.setSceneAntialiasing(GraphicsConfigTemplate.PREFERRED);
        GraphicsConfiguration config = GraphicsEnvironment
                .getLocalGraphicsEnvironment().getDefaultScreenDevice()
                .getBestConfiguration(template);
        // create canvas
        canvas3d = new Canvas3D(config);
        canvas3d.setDoubleBufferEnable(true);
        
        // display j3d info
        Map map = canvas3d.queryProperties();
        System.out.println("doubleBufferAvailable = "
                + map.get("doubleBufferAvailable"));
        System.out.println("sceneAntialiasingNumPasses = "
                + map.get("sceneAntialiasingNumPasses"));
        System.out.println("sceneAntialiasingAvailable = "
                + map.get("sceneAntialiasingAvailable"));
        System.out.println("texture3DAvailable = "
                + map.get("texture3DAvailable"));
        
             
    }



    /** Adds a 3D object to the world. 
     * Used only in the creation phase.*/
      public void addObjectToPickableSceneBranch(BaseObject obj3d) {
         obj3d.compile();
         pickableSceneBranch.addChild(obj3d.getNode());
     }
     
      /** Detach a previously attached object from the scenegraph. */
      public void detach(BaseObject obj3d) {
          pickableSceneBranch.removeChild(obj3d.getNode());
      }
      /** attach a 3d object to the scenegraph. */
      public void attach(BaseObject obj3d) {
          pickableSceneBranch.addChild(obj3d.getNode());
      }

    /** Add a light to the 3d world.   
     * Used only in the creation phase.
     */
    Light addLight(Vector3d pos, Color3f color) {
        BoundingSphere bounds = new BoundingSphere(new Point3d(0.0, 0.0, 0.0),
                worldSize*2);

        TransformGroup tg = new TransformGroup();
        Transform3D t3d = new Transform3D();
        t3d.set(pos);
        tg.setTransform(t3d);
        PointLight light = new PointLight();
        light.setAttenuation(0.5f,0,0);
         // light.setAttenuation(0f,.08f,0);
        // light.setAttenuation(1.2f,0,0);
        // note : light pos not affected by transform (but bound is).
       light.setPosition((float) pos.x, (float) pos.y, (float) pos.z);
       light.setInfluencingBounds(bounds);
        sceneTrans.addChild(light);
        // light geometry
        ColoringAttributes ca = new ColoringAttributes();
        ca.setColor(color);
        Appearance appL1 = new Appearance();
        appL1.setColoringAttributes(ca);
        Primitive s = new Sphere(0.4f, appL1);
        s.setCollidable(true);
        tg.addChild(s);
        sceneTrans.addChild(tg);
        return light;
    }
    
    /** Creates the branch for the visible content of the scenegraph.  
     * Used only in the creation phase.
     */
    private  void createSceneBranch(EnvironmentDescription wd) {
  
   
        sceneBranch = new BranchGroup();
        sceneRot = new TransformGroup();
        sceneRot.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        sceneRot.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        sceneRot.setCapability(TransformGroup.ALLOW_CHILDREN_EXTEND);
        // add transform 
        sceneTrans = new TransformGroup();
        // allow  transform access
        sceneTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        sceneTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        sceneTrans.setCapability(TransformGroup.ALLOW_CHILDREN_EXTEND);
        sceneBranch.addChild(sceneRot);
        sceneRot.addChild(sceneTrans);
        
        // bounds (lights,background, behaviors)
        BoundingSphere bounds = new BoundingSphere(new Point3d(0.0, 0.0, 0.0),
                worldSize*3);