/// <summary> /// Box intersect /// </summary> /// <param name="rayBox">Ray box</param> /// <param name="ray">Ray</param> /// <param name="vertexPositions">Vertex positions</param> /// <param name="distance">Distance</param> /// <param name="collisionPosition">Collision position</param> /// <param name="collisionNormal">Collision normal</param> /// <returns>Bool</returns> public virtual bool DoesBoxIntersect(BoxHelper rayBox, Ray ray, Vector3[] vertexPositions, out float distance, out Vector3 collisionPosition, out Vector3 collisionNormal) { distance = 0; collisionPosition = Vector3.Zero; collisionNormal = Vector3.Zero; return false; }
/// <summary> /// Box move /// </summary> /// <param name="box">Box</param> /// <param name="pointStart">Point start</param> /// <param name="pointEnd">Point end</param> /// <param name="vertexPositions">Vertex positions</param> /// <param name="frictionFactor">Friction factor</param> /// <param name="bumpFactor">Bump mapping factor</param> /// <param name="recurseLevel">Recurse level</param> /// <param name="pointResult">Point result</param> /// <param name="velocityResult">Velocity result</param> public void BoxMove(BoxHelper box, Vector3 pointStart, Vector3 pointEnd, Vector3[] vertexPositions, float frictionFactor, float bumpFactor, uint recurseLevel, out Vector3 pointResult, ref Vector3 velocityResult) { pointResult = pointStart; Vector3 delta = pointEnd - pointStart; float deltaLength = delta.Length(); if (deltaLength < 0.00001f) return; float total_dist = deltaLength; delta *= 1.0f / deltaLength; Vector3 main_dir = delta; while (recurseLevel > 0) { float dist; Vector3 pos, norm; if (false == DoesBoxIntersect(box, Ray.FromStartAndEnd(pointStart, pointEnd), vertexPositions, out dist, out pos, out norm)) { pointStart = pointEnd; break; } // if (false) if (dist > 0.01f) { pointStart += delta * (dist - 0.01f); total_dist -= dist; } // if (dist) Vector3 reflect_dir = Vector3.Reflect(delta, norm); Vector3 n = norm * Vector3.Dot(reflect_dir, norm); Vector3 t = reflect_dir - n; reflect_dir = frictionFactor * t + bumpFactor * n; pointEnd = pointStart + reflect_dir * total_dist; delta = pointEnd - pointStart; deltaLength = delta.Length(); if (deltaLength < 0.00001f) break; delta *= 1.0f / deltaLength; recurseLevel--; } // while (recurseLevel) pointResult = pointStart; velocityResult = delta * velocityResult.Length(); }
/// <summary> /// Create collision polygon /// </summary> /// <param name="offset">Offset</param> /// <param name="vert_indx">Vert _indx</param> /// <param name="vert_offset">Vert _offset</param> /// <param name="vertexPositions">Vert _pos</param> public CollisionPolygon(int offset, int[] vert_indx, int vert_offset, Vector3[] vertexPositions) { vertexIndices = new int[3]; box = new BoxHelper(float.MaxValue, -float.MaxValue); for (int i = 0; i < 3; i++) { vertexIndices[i] = vert_indx[i + offset] + vert_offset; box.AddPoint(vertexPositions[vertexIndices[i]]); } // for (int) }
/// <summary> /// Create collision node /// </summary> /// <param name="b">B</param> /// <param name="subdivLevel">Subdiv _level</param> public CollisionNode(BoxHelper setBox, uint subdivLevel) { box = setBox; if (subdivLevel > 0) { subdivLevel--; childs = new CollisionNode[8]; BoxHelper[] childs_box = box.GetChilds(); for (uint i = 0; i < 8; i++) childs[i] = new CollisionNode(childs_box[i], subdivLevel); } // if (subdivLevel) }
/// <summary> /// Get elements /// </summary> /// <param name="b">B</param> /// <param name="e">E</param> /// <param name="recurse_id">Recurse _id</param> public void GetElements(BoxHelper checkBox, List<BaseCollisionObject> elements, uint id) { if (checkBox.DoesBoxIntersect(box) == false) return; if (elems != null) { foreach (BaseCollisionObject elem in elems) { if (elem.id < id) { if (elem.box.DoesBoxIntersect(checkBox)) elements.Add(elem); elem.id = id; } // if (elem.id) } // foreach (elem) } // if (elems) if (childs != null) { foreach (CollisionNode node in childs) node.GetElements(checkBox, elements, id); } // if (childs) }
/// <summary> /// Box intersect face and return intersection distance, point and normal /// </summary> /// <param name="rayBox">Ray box</param> /// <param name="ray">Ray</param> /// <param name="vertexPositions">Vertex positions</param> /// <param name="distance">Distance</param> /// <param name="collisionPosition">Collision position</param> /// <param name="collisionNormal">Collision normal</param> /// <returns>Bool</returns> public override bool DoesBoxIntersect(BoxHelper rayBox, Ray ray, Vector3[] vertexPositions, out float distance, out Vector3 collisionPosition, out Vector3 collisionNormal) { distance = float.MaxValue; collisionPosition = ray.origin; collisionNormal = Vector3.Zero; bool intersected = false; Vector3 p1, p2, p3, p4; uint i, j; BoxHelper world_box = new BoxHelper(rayBox.min + ray.origin, rayBox.max + ray.origin); Vector3[] box_verts = world_box.GetVertices(); Vector3[] box_edges = world_box.GetEdges(); // Intersect box edges to face edges for (i = 0; i < 12; i++) { // Cull edges with normal more than 135 degree from moving // direction if (Vector3.Dot(BoxHelper.edgeNormals[i], ray.direction) < -0.70710678) continue; p1 = box_edges[i * 2]; p2 = box_edges[i * 2 + 1]; p4 = vertexPositions[vertexIndices[0]]; for (j = 0; j < vertexIndices.Length; j++) { p3 = p4; p4 = vertexPositions[vertexIndices[(j + 1) % vertexIndices.Length]]; float checkDistance; Vector3 position; if (CollisionPolygon.EdgeIntersect(p1, p2, ray.direction, p3, p4, out checkDistance, out position)) { if (checkDistance < distance) { distance = checkDistance; collisionPosition = position; collisionNormal = Vector3.Normalize( Vector3.Cross(p2 - p1, p3 - p4)); if (Vector3.Dot(ray.direction, collisionNormal) > 0) collisionNormal = Vector3.Negate(collisionNormal); intersected = true; } // if (checkDistance) } // if (CollisionTris.EdgeIntersect) } // for (j) } // for (i) // Intersect from face vertices to box for (i = 0; i < 3; i++) { float tnear, tfar; p1 = vertexPositions[vertexIndices[i]]; int box_face_id = world_box.RayIntersect(p1, -ray.direction, out tnear, out tfar); if (box_face_id > -1) { if (tnear < distance) { distance = tnear; collisionPosition = p1; collisionNormal = -BoxHelper.faceNormals[box_face_id]; intersected = true; } // if (tnear) } // if (box_face_id) } // for (i) // Intersect from box vertices to face polygon Vector3 v1 = vertexPositions[vertexIndices[0]]; Vector3 v2 = vertexPositions[vertexIndices[1]]; Vector3 v3 = vertexPositions[vertexIndices[2]]; for (i = 0; i < 8; i++) { // Cull vertices with normal more than 135 degree from moving // direction if (Vector3.Dot(BoxHelper.vertexNormals[i], ray.direction) < -0.70710678) continue; Vector3 uvt; if (CollisionPolygon.RayTriangleIntersect( new Ray(box_verts[i], ray.direction), v1, v2, v3, out uvt.Z, out uvt.X, out uvt.Y)) { if (uvt.Z < distance) { distance = uvt.Z; collisionPosition = (1.0f - uvt.X - uvt.Y) * v1 + uvt.X * v2 + uvt.Y * v3; collisionNormal = Vector3.Normalize( Vector3.Cross(v3 - v1, v2 - v1)); intersected = true; } // if (uvt.Z) } // if (CollisionTris.RayTriangleIntersect) } // for (i) return intersected; }
/// <summary> /// Create collision helper /// </summary> /// <param name="box">Box</param> /// <param name="subdivLevel">Subdiv _level</param> public CollisionHelper(BoxHelper box, uint subdivLevel) { root = new CollisionNode(box, subdivLevel); id = 0; }
/// <summary> /// Get elements /// </summary> /// <param name="box">Box</param> /// <param name="elements">Elements</param> public void GetElements(BoxHelper box, List<BaseCollisionObject> elements) { root.GetElements(box, elements, ++id); }
/// <summary> /// Point intersect /// </summary> /// <param name="ray">Ray</param> /// <param name="vertexPositions">Vertex positions</param> /// <param name="distance">Distance</param> /// <param name="collisionPosition">Collision position</param> /// <param name="collisionNormal">Collision normal</param> /// <returns>Bool</returns> public bool DoesRayIntersect(Ray ray, Vector3[] vertexPositions, out float distance, out Vector3 collisionPosition, out Vector3 collisionNormal) { distance = 0.0f; collisionPosition = ray.origin; collisionNormal = Vector3.Zero; if (ray.Length == 0) return false; BoxHelper rayBox = new BoxHelper(float.MaxValue, -float.MaxValue); rayBox.AddPoint(ray.origin); rayBox.AddPoint(ray.EndPosition); Vector3 inflate = new Vector3(0.001f, 0.001f, 0.001f); rayBox.min -= inflate; rayBox.max += inflate; List<BaseCollisionObject> elems = new List<BaseCollisionObject>(); root.GetElements(rayBox, elems, ++id); ray.direction *= 1.0f / ray.Length; distance = ray.Length; bool intersected = false; foreach (BaseCollisionObject collisionObject in elems) { float checkDistance; Vector3 position; Vector3 normal; if (true == collisionObject.DoesRayIntersect(ray, vertexPositions, out checkDistance, out position, out normal)) { if (checkDistance < distance) { distance = checkDistance; collisionPosition = position; collisionNormal = normal; intersected = true; } // if (checkDistance) } // if (true) } // foreach (collisionObject) return intersected; }
/// <summary> /// Load mesh, must be called after we got all bones. Will also create /// the vertex and index buffers and optimize the vertices as much as /// we can. /// </summary> /// <param name="colladaFile">Collada file</param> private void LoadMesh(XmlNode colladaFile) { XmlNode geometrys = XmlHelper.GetChildNode(colladaFile, "library_geometries"); if (geometrys == null) throw new InvalidOperationException( "library_geometries node not found in collision file"); foreach (XmlNode geometry in geometrys) if (geometry.Name == "geometry") { // Load everything from the mesh node LoadMeshGeometry(colladaFile, XmlHelper.GetChildNode(colladaFile, "mesh"), XmlHelper.GetXmlAttribute(geometry, "name")); // Optimize vertices first and build index buffer from that! indices = OptimizeVertexBuffer(); // Copy and create everything to CollisionFace faces = new CollisionPolygon[indices.Length / 3]; for (int i = 0; i < indices.Length / 3; i++) { faces[i] = new CollisionPolygon(i * 3, indices, 0, vectors); } // for (int) BoxHelper box = new BoxHelper(float.MaxValue, -float.MaxValue); for (int i = 0; i < vectors.Length; i++) box.AddPoint(vectors[i]); uint subdivLevel = 4; // max 8^6 nodes tree = new CollisionHelper(box, subdivLevel); for (int i = 0; i < faces.Length; i++) tree.AddElement(faces[i]); // Get outa here, we currently only support one single mesh! return; } // foreach if (geometry.Name) }
/// <summary> /// Get elements /// </summary> /// <param name="b">B</param> /// <param name="e">E</param> public void GetElements(BoxHelper b, List<BaseCollisionObject> e) { tree.GetElements(b, e); }
/// <summary> /// Box intersect /// </summary> /// <param name="box">Box</param> /// <param name="ray_start">Ray _start</param> /// <param name="ray_end">Ray _end</param> /// <param name="distance">Intersect _distance</param> /// <param name="collisionPosition">Intersect _position</param> /// <param name="collisionNormal">Intersect _normal</param> /// <returns>Bool</returns> public bool DoesBoxIntersect(BoxHelper box, Ray ray, out float distance, out Vector3 collisionPosition, out Vector3 collisionNormal) { return tree.DoesBoxIntersect(box, ray, vectors, out distance, out collisionPosition, out collisionNormal); }
/// <summary> /// Box move /// </summary> /// <param name="box">Box</param> /// <param name="pointStart">Point _start</param> /// <param name="pointEnd">Point _end</param> /// <param name="frictionFactor">Friction _factor</param> /// <param name="bumpFactor">Bump mapping _factor</param> /// <param name="recurseLevel">Recurse _level</param> /// <param name="pointResult">Point _result</param> /// <param name="velocityResult">Velocity _result</param> public void BoxMove(BoxHelper box, Vector3 pointStart, Vector3 pointEnd, float frictionFactor, float bumpFactor, uint recurseLevel, out Vector3 pointResult, ref Vector3 velocityResult) { tree.BoxMove(box, pointStart, pointEnd, vectors, frictionFactor, bumpFactor, recurseLevel, out pointResult, ref velocityResult); }
/// <summary> /// Split in middle point creating 8 childs /// </summary> public BoxHelper[] GetChilds() { Vector3 center = 0.5f * (min + max); BoxHelper[] childs = new BoxHelper[8]; childs[0] = new BoxHelper(min, center); childs[1] = new BoxHelper(new Vector3(center.X, min.Y, min.Z), new Vector3(max.X, center.Y, center.Z)); childs[2] = new BoxHelper(new Vector3(min.X, center.Y, min.Z), new Vector3(center.X, max.Y, center.Z)); childs[3] = new BoxHelper(new Vector3(center.X, center.Y, min.Z), new Vector3(max.X, max.Y, center.Z)); childs[4] = new BoxHelper(new Vector3(min.X, min.Y, center.Z), new Vector3(center.X, center.Y, max.Z)); childs[5] = new BoxHelper(new Vector3(center.X, min.Y, center.Z), new Vector3(max.X, center.Y, max.Z)); childs[6] = new BoxHelper(new Vector3(min.X, center.Y, center.Z), new Vector3(center.X, max.Y, max.Z)); childs[7] = new BoxHelper(center, max); return childs; }
/// <summary> /// Check if two bounding boxes have any intersection /// </summary> /// <param name="bb">Bb</param> /// <returns>Bool</returns> public bool DoesBoxIntersect(BoxHelper bb) { if (max.X >= bb.min.X && min.X <= bb.max.X && max.Y >= bb.min.Y && min.Y <= bb.max.Y && max.Z >= bb.min.Z && min.Z <= bb.max.Z) return true; return false; }
/// <summary> /// Constructor from another collision box /// </summary> /// <param name="bb">box</param> public BoxHelper(BoxHelper bb) { min = bb.min; max = bb.max; }