<> <> <> <> DIRECTORY SVCoordSys, SVGraphics, SVMatrix3d, RealFns, Imager, SV3d, SVBasicTypes, SVBoundBox, SVBoundSphere, SVModelTypes, SVPolygon3d, SVSceneTypes, SVVector3d; SVBoundSphereImpl: CEDAR PROGRAM IMPORTS SVCoordSys, SVGraphics, SVMatrix3d, RealFns, SVBoundBox, SVPolygon3d, SVVector3d EXPORTS SVBoundSphere = BEGIN BoundHedron: TYPE = SVBasicTypes.BoundHedron; Camera: TYPE = SVModelTypes.Camera; Circle3d: TYPE = SV3d.Circle3d; CoordSystem: TYPE = SVModelTypes.CoordSystem; Matrix4by4: TYPE = SV3d.Matrix4by4; Point3d: TYPE = SV3d.Point3d; Poly3d: TYPE = SV3d.Poly3d; Ray: TYPE = SVSceneTypes.Ray; Sphere: TYPE = REF SphereObj; SphereObj: TYPE = SV3d.SphereObj; Vector3d: TYPE = SV3d.Vector3d; gCirclePolygon: Poly3d; PI: REAL = 3.14159265358979; Init: PROC [] = { gCirclePolygon _ PolyOnUnitCircle[20]; }; PolyOnUnitCircle: PROC [edges: NAT] RETURNS [poly: Poly3d] ~ { <> theta: REAL _ 0.0; realEdges: REAL _ edges; delta: REAL _ 2.0*PI/realEdges; x, y: REAL; poly _ SVPolygon3d.CreatePoly[edges]; FOR i: NAT IN [0..edges) DO x _ RealFns.Sin[theta]; y _ RealFns.Cos[theta]; poly _ SVPolygon3d.AddPolyPoint[poly, [x, y, 0]]; theta _ theta + delta; ENDLOOP; }; CopyBoundSphere: PUBLIC PROC [boundSphere: Sphere] RETURNS [newBS: Sphere] = { newBS _ NEW[SphereObj _ [ boundSphere.center, boundSphere.radius, boundSphere.radiusSquared]]; }; BoundSphereFromBoundHedron: PUBLIC PROC [bh: BoundHedron, worldCS: CoordSystem, localCS: CoordSystem] RETURNS [boundSphere: Sphere] = { <> cmWORLD, bhPtWORLD, cmlocal: Point3d; maxIndex: NAT; maxDistSquared, distSquared, radius: REAL; localWorld: Matrix4by4 _ SVCoordSys.WRTWorld[localCS]; IF bh = NIL THEN {boundSphere _ NIL; RETURN}; cmlocal _ SVBoundBox.CenterOfMassBoundHedron[bh]; cmWORLD _ SVMatrix3d.Update[cmlocal, localWorld]; maxIndex _ 0; bhPtWORLD _ SVMatrix3d.Update[bh[0], localWorld]; maxDistSquared _ SVVector3d.MagnitudeSquared[SVVector3d.Sub[cmWORLD, bhPtWORLD]]; FOR i: NAT IN[1..bh.len) DO bhPtWORLD _ SVMatrix3d.Update[bh[i], localWorld]; distSquared _ SVVector3d.MagnitudeSquared[SVVector3d.Sub[cmWORLD, bhPtWORLD]]; IF distSquared > maxDistSquared THEN { maxIndex _ i; maxDistSquared _ distSquared; }; ENDLOOP; radius _ RealFns.SqRt[maxDistSquared]; boundSphere _ NEW[SphereObj _ [center: cmWORLD, radius: radius, radiusSquared: radius*radius]]; }; BoundSphereFromValues: PUBLIC PROC [centerWORLD: Point3d, radius: REAL] RETURNS [boundSphere: Sphere] = { boundSphere _ NEW[SphereObj _ [centerWORLD, radius, radius*radius]]; }; UnionCombineBoundSpheres: PUBLIC PROC [bs1, bs2: Sphere] RETURNS [newBS: Sphere] = { <> d, unitD: Vector3d; almostZero: REAL _ 1.0e-12; magD: REAL; IF bs1 = NIL OR bs2 = NIL THEN {newBS _ NIL; RETURN}; d _ SVVector3d.Sub[bs2.center, bs1.center]; magD _ SVVector3d.Magnitude[d]; <> IF bs1.radius > bs2.radius THEN { IF bs1.radius - magD >= bs2.radius THEN {newBS _ CopyBoundSphere[bs1]; RETURN}; } ELSE { IF bs2.radius - magD >= bs1.radius THEN {newBS _ CopyBoundSphere[bs2]; RETURN}; }; <> IF ABS[magD] < almostZero THEN { IF bs1.radius > bs2.radius THEN {newBS _ CopyBoundSphere[bs1]; RETURN} ELSE {newBS _ CopyBoundSphere[bs2]; RETURN} }; <> newBS _ NEW[SphereObj]; unitD _ SVVector3d.Scale[d, 1.0/magD]; newBS.radius _ (bs1.radius + magD + bs2.radius)/2.0; newBS.radiusSquared _ newBS.radius*newBS.radius; newBS.center _ SVVector3d.Add[bs1.center, bs2.center]; newBS.center _ SVVector3d.Add[newBS.center, SVVector3d.Scale[unitD, bs2.radius - bs1.radius]]; newBS.center _ SVVector3d.Scale[newBS.center, 0.5]; }; IntersectionCombineBoundSpheres: PUBLIC PROC [bs1, bs2: Sphere] RETURNS [newBS: Sphere] = { <> c, cSquared, x, y: REAL; o2minuso1: Vector3d; IF bs1 = NIL AND bs2 = NIL THEN {newBS _ NIL; RETURN}; IF bs1 = NIL THEN {newBS _ CopyBoundSphere[bs2]; RETURN}; IF bs2 = NIL THEN {newBS _ CopyBoundSphere[bs1]; RETURN}; o2minuso1 _ SVVector3d.Sub[bs2.center, bs1.center]; cSquared _ SVVector3d.MagnitudeSquared[o2minuso1]; c _ RealFns.SqRt[cSquared]; IF c > (bs1.radius + bs2.radius) THEN { -- No intersection. Return a radius zero sphere. newBS _ NEW[SphereObj]; newBS.center _ bs1.center; newBS.radius _ 0.0; newBS.radiusSquared _ 0.0; RETURN; }; IF (c + bs1.radius) < bs2.radius THEN { -- bs1 is contained in bs2 newBS _ CopyBoundSphere[bs1]; RETURN; }; IF (c + bs2.radius) < bs1.radius THEN { -- bs2 is contained in bs1 newBS _ CopyBoundSphere[bs2]; }; newBS _ NEW[SphereObj]; x _ (bs1.radiusSquared - bs2.radiusSquared + cSquared)/(2.0*c); y _ RealFns.SqRt[bs1.radiusSquared - x*x]; newBS.center _ SVVector3d.Add[SVVector3d.Scale[o2minuso1, x/c], bs1.center]; newBS.radius _ y; newBS.radiusSquared _ y*y; }; DifferenceCombineBoundSpheres: PUBLIC PROC [bs1, bs2: Sphere] RETURNS [newBS: Sphere] = { <> IF bs1 = NIL THEN {newBS _ NIL; RETURN}; newBS _ CopyBoundSphere[bs1]; }; DrawBoundSphere: PUBLIC PROC [dc: Imager.Context, boundSphere: Sphere, camera: Camera] = { <> circleMat, unitCircleMat: Matrix4by4; circlePoly: Poly3d; rad: REAL; centerToEyepoint: Vector3d; centerCAMERA: Point3d; worldCS: CoordSystem; eyepointCAMERA: Point3d _ [0, 0, camera.focalLength]; IF boundSphere = NIL THEN RETURN; rad _ boundSphere.radius; worldCS _ SVCoordSys.Parent[camera.coordSys]; centerCAMERA _ SVCoordSys.FromCSToCS[boundSphere.center, worldCS, camera.coordSys]; centerToEyepoint _ SVVector3d.VectorFromPoints[centerCAMERA, eyepointCAMERA]; unitCircleMat _ MakeAlignedMat[centerToEyepoint, centerCAMERA]; circleMat _ SVMatrix3d.LocalScale[unitCircleMat, rad, rad, rad]; circlePoly _ SVPolygon3d.TransformByMat[gCirclePolygon, circleMat]; SVGraphics.DrawPolygonAbsolute[dc, circlePoly, 1, butt, camera]; }; DrawCircle3d: PUBLIC PROC [dc: Imager.Context, circle: Circle3d, camera: Camera] = { circleMat, unitCircleMat: Matrix4by4; circlePoly: Poly3d; rad: REAL; normal: Vector3d; centerCAMERA: Point3d; worldCS: CoordSystem; eyepointCAMERA: Point3d _ [0, 0, camera.focalLength]; IF circle = NIL THEN RETURN; rad _ circle.radius; worldCS _ SVCoordSys.Parent[camera.coordSys]; centerCAMERA _ SVCoordSys.FromCSToCS[circle.origin, worldCS, camera.coordSys]; normal[1] _ circle.plane.A; normal[2] _ circle.plane.B; normal[3] _ circle.plane.C; unitCircleMat _ MakeAlignedMat[normal, centerCAMERA]; circleMat _ SVMatrix3d.LocalScale[unitCircleMat, rad, rad, rad]; circlePoly _ SVPolygon3d.TransformByMat[gCirclePolygon, circleMat]; SVGraphics.DrawPolygonAbsolute[dc, circlePoly, 1, butt, camera]; }; MakeAlignedMat: PRIVATE PROC [zaxis: Vector3d, origin: Point3d] RETURNS [mat: Matrix4by4] = { <> xAxis: Vector3d; yAxisOfCamera: Vector3d _ [0,1,0]; IF SVVector3d.Parallel[yAxisOfCamera, zaxis] THEN xAxis _ [1,0,0] ELSE xAxis _ SVVector3d.CrossProduct[yAxisOfCamera, zaxis]; mat _ SVMatrix3d.MakeMatFromZandXAxis[zaxis, xAxis, origin]; }; Init[]; END.