Package jinngine.geometry

Source Code of jinngine.geometry.ConvexHull

/**
* Copyright (c) 2008-2010  Morten Silcowitz.
*
* This file is part of the Jinngine physics library
*
* Jinngine is published under the GPL license, available
* at http://www.gnu.org/copyleft/gpl.html.
*/
package jinngine.geometry;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import quickhull3d.Point3d;
import quickhull3d.QuickHull3D;

import jinngine.geometry.util.MassProperties;
import jinngine.math.InertiaMatrix;
import jinngine.math.Matrix3;
import jinngine.math.Matrix4;
import jinngine.math.Transforms;
import jinngine.math.Vector3;
import jinngine.physics.Body;

/**
* Convex hull geometry implementation, given by a user defined set of points
*/
public class ConvexHull implements SupportMap3, Geometry, Material {

  private final List<Vector3[]> faces = new ArrayList<Vector3[]>();
  private final ArrayList<Vector3> vertices = new ArrayList<Vector3>();
  private final ArrayList<ArrayList<Integer>> adjacent;
  private final ArrayList<Vector3> dualvertices = new ArrayList<Vector3>();
  private final ArrayList<ArrayList<Integer>> dualadjacent;
  private final Vector3 centreOfMass;
  private final double referenceMass;
  private final int numberOfVertices;
  private int cachedVertex = 0;
 
  // Material
  private double friction = 0.5;
  private double restitution = 0.7;
 
  /**
   * Computes vertex adjacency lists. Method simply runs through all faces, which are given as lists of vertex indices, and fills out
   * adjacency lists along the way. It also roots out duplicate adjacency entries, arising from the same pair of vertices being present
   * two adjacent faces. The motivation for this approach is that the face index lists are the only adjacency information available from
   * the QuickHull3D implementation.
   */
  private final ArrayList<ArrayList<Integer>> adjacencyList( int[][] faceindices, int numberOfVertices) {
    ArrayList<ArrayList<Integer>> adjacent = new ArrayList<ArrayList<Integer>>();
   
    //create array of arrays for adjacency lists
    adjacent.ensureCapacity(numberOfVertices);
    for (int i=0;i<numberOfVertices; i++)
      adjacent.add(i, new ArrayList<Integer>());
   
    // for each face
    for ( int[] face : faceindices) {
      //for each vertex
      int prevvertex = face[face.length-1];
      for (int i=0; i<face.length; i++) {
        //add both ways in adjacency list
        int vertex = face[i];
       
        // first vertex (if not already there)
        List<Integer> adj = adjacent.get(prevvertex);
        boolean found = false;
        for (int a : adj)
          if (a == vertex)
            found = true;
        if (!found)
          adj.add(vertex);
       
        // second vertex
        adj = adjacent.get(vertex);
        found = false;
        for (int a : adj)
          if (a == prevvertex)
            found = true;
        if (!found)
          adj.add(prevvertex);

        // set next previous vertex
        prevvertex = vertex;
      }     
    }
   
    return adjacent;
  }
 
  /**
   * Create a convex hull geometry, based on the points given
   * @param input
   */
  public ConvexHull(List<Vector3> input) { 
    final QuickHull3D dualhull = new QuickHull3D();
    final QuickHull3D hull = new QuickHull3D();

    // convert points  
    int i=0; double[] vectors = new double[3*input.size()];
    for (Vector3 v: input) {
      vectors[i+0] = v.x;
      vectors[i+1] = v.y;
      vectors[i+2] = v.z;
      i = i+3;
    }
   
    // build the hull
    hull.build(vectors);
       
    // extract faces from the QuickHull3D implementation
    Point3d[] points = hull.getVertices();
    int[][] faceIndices = hull.getFaces();
   
    // get hull vertices
    for ( Point3d p: points)
      vertices.add(new Vector3( p.x, p.y, p.z));
 
    // grab the number of vertices
    numberOfVertices = vertices.size();
   
    // adjacency lists for hull
    adjacent = adjacencyList(faceIndices, numberOfVertices );
   
    // go thru all faces to make the dual hull points
    for (i=0; i< faceIndices.length; i++) {  
      //convert to Vector3 array
      Vector3[] f = new Vector3[faceIndices[i].length];
      for (int j=0; j<faceIndices[i].length; j++) {
        //Point3d p = points[faceIndices[i][j]]; 
        //f[j] = new Vector3(p.x,p.y,p.z );
        // use the vectors in vertices insted of new vectors
        f[j] = vertices.get(faceIndices[i][j]);
      }

      //append face to external representation
      faces.add(f);
     
      //get face vertices
      Vector3 v1 = f[0];
      Vector3 v2 = f[1];
      Vector3 v3 = f[2];

      // set normal
      Vector3 normal = v1.sub(v2).cross(v3.sub(v2)).normalize().multiply(-1);
     
      // add to the dual polygon vertices (index corresponds to a face)
      dualvertices.add(normal);
    }
   
    // create the dual hull
    i=0; double[] dualvectors = new double[3*dualvertices.size()];
    for (Vector3 v: dualvertices) {
      dualvectors[i+0] = v.x;
      dualvectors[i+1] = v.y;
      dualvectors[i+2] = v.z;
      i = i+3;
    }
   
    // build the dual hull
    dualhull.build(dualvectors);
   
    // create an adjacency list for the dual hull
    dualadjacent = adjacencyList(dualhull.getFaces(), dualvertices.size());     
         
    // perform approximate mass calculation
    MassProperties masscalculation = new MassProperties( this, 0.05 );
   
    // set propperties
    mass = referenceMass = masscalculation.getMass();
    inertiamatrix = new InertiaMatrix(masscalculation.getInertiaMatrix());
    centreOfMass = masscalculation.getCentreOfMass();

    // align all vertices and faces to centre of mass coordinates
    for ( Vector3 p: vertices)
      Vector3.add( p, centreOfMass.multiply(-1) );
   
//    for ( Vector3[] f: faces)
//      for (Vector3 p: f)
//        Vector3.add( p, centreOfMass.multiply(-1));

   
    //search vertices to find extremal bounds
    extremalpoint.assignZero();
    for ( Vector3 v: vertices) {
      if (extremalpoint.norm() < v.norm()) extremalpoint.assign(v);
    }
   
    // assign max extends ( assume identity scale at this point)
    double max = extremalpoint.norm();
    bounds.assign(max,max,max);

  }
 
  /**
   * Get the number of vertices on the this convex hull
   * @return
   */
  public final int getNumberOfVertices() {
    return vertices.size();
  }
 
  /**
   * Get the vertices of this convex hull in object space
   * @return
   */
  public final Iterator<Vector3> getVertices() {
    return vertices.iterator();
  }
 
  /**
   * Get the faces of the convex hull in object space
   * @return
   */
  public final Iterator<Vector3[]> getFaces() {
    return faces.iterator();
  }

  // Geometry
  private Object auxiliary;
  private Body body = new Body("default");
  private double envelope = 0.125;
  private Matrix3 localrotation = Matrix3.identity();
  private Matrix4 localtransform4 = Matrix4.identity();
  private final Vector3 localtranslation = new Vector3();
//  private final Vector3 displacement = new Vector3();
  private final Vector3 localscale = new Vector3(1,1,1);
  private final Vector3 inverselocalscale = new Vector3(1,1,1);

  // AxisAlignedBoundingBox
  private final Vector3 extremalpoint = new Vector3();
  private final Vector3 bounds = new Vector3();
//  private final Vector3 minBounds = new Vector3();
//  private final Vector3 maxBounds = new Vector3();
//  private final Vector3 minBoundsTransformed = new Vector3();
//  private final Vector3 maxBoundsTransformed = new Vector3();
 
  // Material
  private double mass = 1;
  private final InertiaMatrix inertiamatrix;
 
  @Override
  public Vector3 supportPoint(Vector3 direction) {
    // normals are transformed (RS^-1)
    Vector3 v = body.state.rotation.multiply(localrotation).scale(localscale).transpose().multiply(direction);
   
    // do hill climbing if the hull has a considerable number of vertices
//    if (numberOfVertices > 32) {
      //hill climb along v
      int index = cachedVertex;
      double value = v.dot(vertices.get(index));
      boolean better = true;
      while (better) {
        better = false;
        //go through adjacency list and pick first improver (greedy)
        for ( int i: adjacent.get(index)) {
          double newvalue = v.dot(vertices.get(i));
          if ( newvalue > value) {
            value = newvalue;
            index = i;
            better = true;
            break;
          }
        }
      }
     
      // keep the vertex
      cachedVertex = index;
     
      // return the final support point in world space
      return body.state.rotation.multiply(localrotation.scale(localscale).multiply(vertices.get(index)).add(localtranslation)).add(body.state.position);

//    } else {
//      // if not, just check each vertex
//      double value = Double.NEGATIVE_INFINITY;
//      Vector3 best = null;
//      for (Vector3 p: vertices) {
//        if (v.dot(p) > value || best == null) {
//          best = p;
//          value = v.dot(p);
//        }
//      }
//     
//      // return final support point in world space
//      return body.state.rotation.multiply(localrotation.multiply(best).add(localtranslation)).add(body.state.position);     
//    }

  }

  @Override
  public void supportFeature(Vector3 direction, List<Vector3> returnface) {
    // Su 
    // the support feature of CovexHull is the face with the face normal closest to the
    // given support direction. This is accomplished by hill-climbing the dual hull, that
    // maps a vertex onto a face in the original convex hull. Therefore, this method will
    // return a face at all times
    Vector3 v = body.state.rotation.multiply(localrotation).scale(localscale).transpose().multiply(direction);
    // hill climb the dual hull to find face
    int index = 0;
    double value = v.dot(dualvertices.get(index));
    boolean better = true;
    while (better) {
      better = false;
      //go through adjacency list and pick first improver (greedy)
      for ( int i: dualadjacent.get(index)) {
        double newvalue = v.dot(dualvertices.get(i));
        if ( newvalue > value) {
          value = newvalue;
          index = i;
          better = true;
          break;
        }
      }
    }
   
    // output the face according to the dual hull index
    for (Vector3 p: faces.get(index))
      returnface.add( body.state.rotation.multiply(localrotation.scale(localscale).multiply(p).add(localtranslation)).add(body.state.position) );
  }

 
  @Override
  public Body getBody() {
    return body;
  }

  @Override
  public double getEnvelope() {
    return envelope;
  }

  @Override
  public InertiaMatrix getInertialMatrix() {
    // scale the inertia matrix in the specified mass and reference mass ratio
    return new InertiaMatrix(inertiamatrix.multiply( mass / referenceMass ));
  }

   
  @Override
  public Matrix4 getTransform() {
    return Matrix4.multiply(body.getTransform(), localtransform4, new Matrix4());
  }

  @Override
  public void setBody(Body b) {
    this.body = b;
  }

  @Override
  public void setEnvelope(double envelope) {
    this.envelope = envelope;
  }

  @Override
  public void setLocalTransform(Matrix3 localRot, Vector3 localTransl) {
    this.localtranslation.assign(localTransl);
                this.localrotation.assign(localRot);
                localtransform4.assign( Transforms.transformAndTranslate4(localrotation, localtranslation) );
  }
 
  @Override
  public Vector3 getMaxBounds() {
    // return the max bounds in world space
//    return bounds.add(localtranslation).add(body.state.position);   
    return body.state.rotation.multiply(localtranslation).add(body.state.position).add(bounds);

  }

  @Override
  public Vector3 getMinBounds() {
    // return the min bounds in world space
    return body.state.rotation.multiply(localtranslation).add(body.state.position).sub(bounds);
  }

  @Override
  public void getLocalTransform(Matrix3 R, Vector3 b) {
    R.assign(this.localrotation);
    b.assign(this.localtranslation);   
  }

  @Override
  public double getMass() {
    return mass;
  }

  public void setMass(double m) {
    this.mass = m;
  }

  @Override
  public void getLocalTranslation(Vector3 t) {
    t.assign(localtranslation);
  }
 
  @Override
  public Object getAuxiliary() {
    return auxiliary;
  }

  @Override
  public void setAuxiliary(Object auxiliary) {
    this.auxiliary = auxiliary;
  }
 
  public Vector3 getCentreOfMass() {
    return new Vector3(centreOfMass);
  }

  @Override
  public double getFrictionCoefficient() {
    return this.friction;
  }

  @Override
  public double getRestitution() {
    return this.restitution;
  }

  @Override
  public void setFrictionCoefficient(double f) {
    this.friction = f;
  }

  @Override
  public void setRestitution(double e) {
    this.restitution = e;
  }

  @Override
  public void setLocalScale(Vector3 s) {
    localscale.assign(s);
    inverselocalscale.assign(1/s.x,1/s.y,1/s.z);
   
    //re-assign bounds by scaling the previous extremal point TODO not sure this is tight
    //search vertices to find extremal bounds
    extremalpoint.assignZero();
    for ( Vector3 v: vertices) {
      if (extremalpoint.norm() < v.scale(localscale).norm())
        extremalpoint.assign(v.scale(localscale));
    }
    double max = extremalpoint.norm();
    bounds.assign(max,max,max);
    System.out.println("max="+max);

  }

  @Override
  public double sphereSweepRadius() {
    return 0;
  }
}
TOP

Related Classes of jinngine.geometry.ConvexHull

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.