fix bugs

src/ja3d/collisions/EllipsoidCollider.java

@@ -64,8 +64,8 @@ public class EllipsoidCollider {
 
 	public EllipsoidCollider(double radiusX, double radiusY, double radiusZ) {
 		this.radiusX = radiusX;
-		this.radiusX = radiusY;
-		this.radiusX = radiusZ;
+		this.radiusY = radiusY;
+		this.radiusZ = radiusZ;
 	}
 
 	public void calculateSphere(Transform3D transform) {
@@ -76,7 +76,7 @@ public void calculateSphere(Transform3D transform) {
 		// sphere.w, the radius is the max distance from the sphere to the four corners.
 		Vector3D s = new Vector3D();
 		Vector3D d = new Vector3D();
-		Vector3D[] corners = {cornerA, cornerD, cornerD, cornerD};
+		Vector3D[] corners = {cornerA, cornerB, cornerC, cornerD};
 		for (Vector3D corner : corners) {
 			transform.transform(corner, s);
 			substract(s, sphere, d);
@@ -106,7 +106,7 @@ private void prepare(Vector3D source, Vector3D displacement,
 		// Local source - src
 		src.setTo(0, 0, 0);
 		// Local offset - displ
-		matrix.transformWithoutTranslate(displacement, displ);
+		inverseMatrix.transformWithoutTranslate(displacement, displ);
 		// Local destination point - dest
 		add(src, displ, dest);
 
@@ -220,6 +220,7 @@ private void prepare(Vector3D source, Vector3D displacement,
 				normals.add(normal.y);
 				normals.add(normal.z);
 				normals.add(offset);
+				numTriangles ++;
 			}
 			// Offset by number of vertices
 			mapOffset += geometry.getNumVertices();
@@ -418,7 +419,7 @@ private boolean checkCollision() {
 				// Length of Vector pointed from closest point to the center of sphere
 				double deltaLength = delta.getLengthSquared();
 				double projectionLength = dotProduct(delta, back);
-				double projectionInsideLength = radius * radius - deltaLength - projectionLength * projectionLength;
+				double projectionInsideLength = radius * radius - deltaLength + projectionLength * projectionLength;
 				if (projectionInsideLength > 0) {
 					// Time of the intersection
 					double time = (projectionLength - Math.sqrt(projectionInsideLength)) / displacementLength;

src/ja3d/core/Object3D.java

@@ -11,17 +11,17 @@ public class Object3D {
 
 	private BoundBox boundBox;
 
-	private double x;
-	private double y;
-	private double z;
+	private double x = 0;
+	private double y = 0;
+	private double z = 0;
 
-	private double rotationX;
-	private double rotationY;
-	private double rotationZ;
+	private double rotationX = 0;
+	private double rotationY = 0;
+	private double rotationZ = 0;
 
-	private double scaleX;
-	private double scaleY;
-	private double scaleZ;
+	private double scaleX = 1;
+	private double scaleY = 1;
+	private double scaleZ = 1;
 
 	private Object3D parent;
 	private Object3D childrenList;

src/ja3d/loaders/Parser.java

@@ -15,4 +15,8 @@ protected void init() {
 		objects = new ArrayList<Object3D>();
 	}
 
+	public List<Object3D> getHierarchy() {
+		return hierarchy;
+	}
+
 }

src/ja3d/loaders/collada/DaePrimitive.java

@@ -215,19 +215,24 @@ public int fillGeometry(Geometry geometry, List<DaeVertex> vertices) {
 		indexBegin = geometry._indices.size();
 		for (int i = 0; i < numIndices; i+= inputsStride) {
 			int index = indices[i + mainInput.offset()];
-
 			DaeVertex vertex = index < vertices.size() ? vertices.get(index) : null;
 			// not exist
 			if (vertex == null || !isEqual(vertex, indices, i, inputOffsets)) {
-				vertex = new DaeVertex();
-				// replace or add to end
-				if (index < vertices.size()) {
-					vertices.set(index, vertex);
-				} else {
-					vertices.add(vertex);
+
+				DaeVertex newVertex = new DaeVertex();
+
+				if (vertex != null) {
+					vertices.add(newVertex);
 					index = vertices.size() - 1;
+				} else {
+					while(vertices.size() <= index) {
+						vertices.add(null);
+					}
+					vertices.set(index, newVertex);
 				}
 
+				vertex = newVertex;
+
 				vertex.vertexInIndex = indices[i + verticesInput.offset()];
 				vertex.addPosition(positionSource.numbers, vertex.vertexInIndex, positionSource.stride, document.unitScaleFactor);
 

src/test/Main.java

@@ -2,8 +2,12 @@
 
 import java.io.File;
 import java.io.IOException;
+import java.util.List;
 
 import ja3d.loaders.ParserCollada;
+import ja3d.objects.Mesh;
+import ja3d.collisions.EllipsoidCollider;
+import ja3d.core.*;
 
 import javax.xml.parsers.DocumentBuilder;
 import javax.xml.parsers.DocumentBuilderFactory;
@@ -12,17 +16,69 @@
 import org.w3c.dom.Document;
 import org.xml.sax.SAXException;
 
+import utils.Vector3D;
+
 public class Main {
 
+	private static Object3D _model;
+
 	public static void main(String[] args) 
 			throws ParserConfigurationException, SAXException, IOException {
 
 		DocumentBuilder builder = DocumentBuilderFactory.newInstance().newDocumentBuilder();
-		Document document = builder.parse(new File("src/test/xiangzi.xml"));
+		Document document = builder.parse(new File("src/test/XIANGZI.xml"));
 
 		ParserCollada p = new ParserCollada();
 		p.parse(document, "", false);
 
+		initFromDae(p);
+
+
+		EllipsoidCollider ellipsoid = new EllipsoidCollider(40, 40, 40);
+
+		Vector3D src = new Vector3D(-10, -10, 40);
+		Vector3D displ = new Vector3D(30, 30, 0);
+		Vector3D collidePoint = new Vector3D();
+		Vector3D collidePlane = new Vector3D();
+
+		boolean collided = ellipsoid.getCollision(src, displ, collidePoint, collidePlane, _model, null);
+
+		System.out.println(collided);
+	}
+
+
+
+
+	private static void initFromDae(ParserCollada daeParser) {
+		_model = new Object3D();
+		_model.setName("ground");
+
+		for (Object3D o : daeParser.getHierarchy()) {
+//			if (!(o instanceof Mesh)) {
+//				switch(o.numChildren) {
+//					case 1:
+//						Object3D child = o.getChildAt(0);
+//						if (child.getName().equals(o.getName() + "_PIVOT"))) { // add PIVOT node directly into the ground.
+//							child.setName(o.getName());
+//							child.matrix = child.concatenatedMatrix;
+//							o = child;
+//						} else {
+//							trace(o.name + " has an unexpected child named " + child.name);
+//							o = null;
+//						}
+//						break;
+//					default:
+//						trace(o.name + " has unexpected " + o.numChildren + " object");
+//						o = null;
+//						break;
+//				}
+//			}
+			if (o != null) {
+				// o now is a mesh
+				Mesh oo = (Mesh)o;
+				_model.addChild(oo);	
+			}
+		}
 	}
 
 }