/// <summary> /// Gets the intersection between the box and the ray. /// </summary> /// <param name="ray">Ray to test against the box.</param> /// <param name="transform">Transform of the shape.</param> /// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param> /// <param name="hit">Hit data for the raycast, if any.</param> /// <returns>Whether or not the ray hit the target.</returns> public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { hit = new RayHit(); Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Vector3 localOrigin; Vector3.Subtract(ref ray.Position, ref transform.Position, out localOrigin); Quaternion.Transform(ref localOrigin, ref conjugate, out localOrigin); Vector3 localDirection; Quaternion.Transform(ref ray.Direction, ref conjugate, out localDirection); Vector3 normal = Toolbox.ZeroVector; float temp, tmin = 0, tmax = maximumLength; if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth)) return false; float inverseDirection = 1 / localDirection.X; float t1 = (-halfWidth - localOrigin.X) * inverseDirection; float t2 = (halfWidth - localOrigin.X) * inverseDirection; var tempNormal = new Vector3(-1, 0, 0); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal.Invert( out tempNormal ); } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) normal = tempNormal; tmax = Math.Min(tmax, t2); if (tmin > tmax) return false; if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight)) return false; inverseDirection = 1 / localDirection.Y; t1 = (-halfHeight - localOrigin.Y) * inverseDirection; t2 = (halfHeight - localOrigin.Y) * inverseDirection; tempNormal = new Vector3(0, -1, 0); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal.Invert( out tempNormal ); } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) normal = tempNormal; tmax = Math.Min(tmax, t2); if (tmin > tmax) return false; if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength)) return false; inverseDirection = 1 / localDirection.Z; t1 = (-halfLength - localOrigin.Z) * inverseDirection; t2 = (halfLength - localOrigin.Z) * inverseDirection; tempNormal = new Vector3(0, 0, -1); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal.Invert( out tempNormal ); } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) normal = tempNormal; tmax = Math.Min(tmax, t2); if (tmin > tmax) return false; hit.T = tmin; Vector3.Multiply(ref ray.Direction, tmin, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Quaternion.Transform(ref normal, ref transform.Orientation, out normal); hit.Normal = normal; return true; }
/// <summary> /// Constructs a compound collidable containing only the specified subset of children. /// </summary> /// <param name="shape">Shape to base the compound collidable on.</param> /// <param name="childIndices">Indices of child shapes from the CompoundShape to include in the compound collidable.</param> /// <returns>Compound collidable containing only the specified subset of children.</returns> public static CompoundCollidable CreatePartialCompoundCollidable(CompoundShape shape, IList<int> childIndices) { if (childIndices.Count == 0) throw new ArgumentException("Cannot create a compound from zero shapes."); CompoundCollidable compound = new CompoundCollidable(); Vector3 center = new Vector3(); float totalWeight = 0; for (int i = 0; i < childIndices.Count; i++) { //Create and add the child object itself. var entry = shape.shapes[childIndices[i]]; compound.children.Add(new CompoundChild(shape, entry.Shape.GetCollidableInstance(), childIndices[i])); //Grab its entry to compute the center of mass of this subset. Vector3 toAdd; Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd); Vector3.Add(ref center, ref toAdd, out center); totalWeight += entry.Weight; } if (totalWeight <= 0) { throw new ArgumentException("Compound has zero total weight; invalid configuration."); } Vector3.Divide(ref center, totalWeight, out center); //Our subset of the compound is not necessarily aligned with the shape's origin. //By default, an object will rotate around the center of the collision shape. //We can't modify the shape data itself since it could be shared, which leaves //modifying the local position of the collidable. //We have the subset position in shape space, so pull the collidable back into alignment //with the origin. //This approach matches the rest of the CompoundHelper's treatment of subsets. Vector3 tmp; center.Invert(out tmp); compound.LocalPosition = tmp; //Recompute the hierarchy for the compound. compound.hierarchy.Tree.Reconstruct(compound.children); compound.Shape = shape; return compound; }