public HashSet <CubicSpace> GetCubicSpaces(PointD position, Vector direction) { PointD ssP = GetStanderCoordinate(position); Vector ssD = Normalize(GetStanderVector(direction)); HashSet <CubicSpace> IntersectedCubicSpace = new HashSet <CubicSpace>(); int[] Dquadrant = Sign(direction.ToArray()); int[] startIndex = GetCubicSpaceIndex(position); int[] nextIndex = GetCubicSpaceIndex(position); PointD nextsP = ssP.Clone() as PointD; if (startIndex[0] > 0 && startIndex[0] < CubeArrayDimentions[0] && startIndex[1] > 0 && startIndex[1] < CubeArrayDimentions[1] && startIndex[2] > 0 && startIndex[2] < CubeArrayDimentions[2]) { IntersectedCubicSpace.Add(this.GetCubicSpace(startIndex)); } bool end = false; while (!end) { double[] rate6 = new double[] { (System.Math.Ceiling(nextsP.X) - nextsP.X), (System.Math.Ceiling(nextsP.Y) - nextsP.Y), (System.Math.Ceiling(nextsP.Z) - nextsP.Z), (System.Math.Floor(nextsP.X) - nextsP.X), (System.Math.Floor(nextsP.Y) - nextsP.Y), (System.Math.Floor(nextsP.Z) - nextsP.Z) }; rate6[0] = ssD.X != 0 ? rate6[0] / ssD.X : double.NegativeInfinity; rate6[1] = ssD.Y != 0 ? rate6[1] / ssD.Y : double.NegativeInfinity; rate6[2] = ssD.Z != 0 ? rate6[2] / ssD.Z : double.NegativeInfinity; rate6[3] = ssD.X != 0 ? rate6[3] / ssD.X : double.NegativeInfinity; rate6[4] = ssD.Y != 0 ? rate6[4] / ssD.Y : double.NegativeInfinity; rate6[5] = ssD.Z != 0 ? rate6[5] / ssD.Z : double.NegativeInfinity; double maxRate = rate6[0]; int maxRateIndex = 0; for (int i = 0; i < 6; i++) { if (rate6[i] > maxRate) { maxRate = rate6[i]; maxRateIndex = i; } } double minRate = maxRate; int minRateIndex = maxRateIndex; for (int i = 0; i < 6; i++) { if (rate6[i] < minRate && rate6[i] >= 0) { minRate = rate6[i]; minRateIndex = i; } } if (minRateIndex == 0 || minRateIndex == 3) { nextIndex[0] += Dquadrant[0]; } if (minRateIndex == 1 || minRateIndex == 4) { nextIndex[1] += Dquadrant[1]; } if (minRateIndex == 2 || minRateIndex == 5) { nextIndex[2] += Dquadrant[2]; } nextsP = nextsP + ssD * (maxRate + minRate) / 2.0; if (nextIndex[0] > 0 && nextIndex[0] < CubeArrayDimentions[0] && nextIndex[1] > 0 && nextIndex[1] < CubeArrayDimentions[1] && nextIndex[2] > 0 && nextIndex[2] < CubeArrayDimentions[2]) { IntersectedCubicSpace.Add(this.GetCubicSpace(nextIndex));// } else if ( ((nextIndex[0] < 0 || nextIndex[0] >= CubeArrayDimentions[0]) && nextIndex[0] / Dquadrant[0] < 0) && ((nextIndex[1] < 0 || nextIndex[1] >= CubeArrayDimentions[1]) && nextIndex[1] / Dquadrant[1] < 0) && ((nextIndex[2] < 0 || nextIndex[2] >= CubeArrayDimentions[2]) && nextIndex[2] / Dquadrant[2] < 0)) { } else { end = true; } } return(IntersectedCubicSpace); }
public HashSet<CubicSpace> GetCubicSpaces(PointD position, Vector direction) { PointD ssP = GetStanderCoordinate(position); Vector ssD = Normalize(GetStanderVector(direction)); HashSet<CubicSpace> IntersectedCubicSpace = new HashSet<CubicSpace>(); int[] Dquadrant = Sign(direction.ToArray()); int[] startIndex = GetCubicSpaceIndex(position); int[] nextIndex = GetCubicSpaceIndex(position); PointD nextsP = ssP.Clone() as PointD; if (startIndex[0] > 0 && startIndex[0] < CubeArrayDimentions[0] && startIndex[1] > 0 && startIndex[1] < CubeArrayDimentions[1] && startIndex[2] > 0 && startIndex[2] < CubeArrayDimentions[2]) IntersectedCubicSpace.Add(this.GetCubicSpace(startIndex)); bool end = false; while (!end) { double[] rate6 = new double[]{ (System.Math.Ceiling(nextsP.X) - nextsP.X), (System.Math.Ceiling(nextsP.Y) - nextsP.Y), (System.Math.Ceiling(nextsP.Z) - nextsP.Z), (System.Math.Floor(nextsP.X) - nextsP.X), (System.Math.Floor(nextsP.Y) - nextsP.Y), (System.Math.Floor(nextsP.Z) - nextsP.Z) }; rate6[0] = ssD.X != 0 ? rate6[0] / ssD.X : double.NegativeInfinity; rate6[1] = ssD.Y != 0 ? rate6[1] / ssD.Y : double.NegativeInfinity; rate6[2] = ssD.Z != 0 ? rate6[2] / ssD.Z : double.NegativeInfinity; rate6[3] = ssD.X != 0 ? rate6[3] / ssD.X : double.NegativeInfinity; rate6[4] = ssD.Y != 0 ? rate6[4] / ssD.Y : double.NegativeInfinity; rate6[5] = ssD.Z != 0 ? rate6[5] / ssD.Z : double.NegativeInfinity; double maxRate = rate6[0]; int maxRateIndex = 0; for (int i = 0; i < 6; i++) if (rate6[i] > maxRate) { maxRate = rate6[i]; maxRateIndex = i; } double minRate = maxRate; int minRateIndex = maxRateIndex; for (int i = 0; i < 6; i++) if (rate6[i] < minRate && rate6[i] >= 0) { minRate = rate6[i]; minRateIndex = i; } if (minRateIndex == 0 || minRateIndex == 3) nextIndex[0] += Dquadrant[0]; if (minRateIndex == 1 || minRateIndex == 4) nextIndex[1] += Dquadrant[1]; if (minRateIndex == 2 || minRateIndex == 5) nextIndex[2] += Dquadrant[2]; nextsP = nextsP + ssD * (maxRate + minRate) / 2.0; if (nextIndex[0] > 0 && nextIndex[0] < CubeArrayDimentions[0] && nextIndex[1] > 0 && nextIndex[1] < CubeArrayDimentions[1] && nextIndex[2] > 0 && nextIndex[2] < CubeArrayDimentions[2]) IntersectedCubicSpace.Add(this.GetCubicSpace(nextIndex));// else if ( ((nextIndex[0] < 0 || nextIndex[0] >= CubeArrayDimentions[0]) && nextIndex[0] / Dquadrant[0] < 0) && ((nextIndex[1] < 0 || nextIndex[1] >= CubeArrayDimentions[1]) && nextIndex[1] / Dquadrant[1] < 0) && ((nextIndex[2] < 0 || nextIndex[2] >= CubeArrayDimentions[2]) && nextIndex[2] / Dquadrant[2] < 0)) { } else end = true; } return IntersectedCubicSpace; }
public HashSet<CubicSpace> GetRay(PointD position, Vector direction) { PointD ssP = GetStanderCoordinate(position); Vector ssD = Normalize(GetStanderVector(direction)); int[] Dquadrant = Sign(direction.ToArray()); int[] startIndex = GetCubicSpaceIndex(position); int[] currentIndex = GetCubicSpaceIndex(position); HashSet<CubicSpace> Ray = new HashSet<CubicSpace>(); #region XYZ射線距離計算 { double Rate1 = Dot(ssD, new Vector(1, 0, 0));////////////////// double Rate2 = Dot(ssD, new Vector(0, 1, 0));////////////////// double Rate3 = Dot(ssD, new Vector(0, 0, 1));////////////////// double v1 = Dquadrant[0] != 0 ? Rate1 * Abs(currentIndex[0] + Dquadrant[0] - ssP[0]) : double.PositiveInfinity; double v2 = Dquadrant[1] != 0 ? Rate2 * Abs(currentIndex[1] + Dquadrant[1] - ssP[1]) : double.PositiveInfinity; double v3 = Dquadrant[2] != 0 ? Rate3 * Abs(currentIndex[2] + Dquadrant[2] - ssP[2]) : double.PositiveInfinity; while( !((currentIndex[0] < 0 && Dquadrant[0] < 0)|| (currentIndex[0] > CubeArrayLastIndex[0] && Dquadrant[0] > 0)) && !((currentIndex[1] < 0 && Dquadrant[1] < 0)|| (currentIndex[1] > CubeArrayLastIndex[1] && Dquadrant[1] > 0)) && !((currentIndex[2] < 0 && Dquadrant[2] < 0)|| (currentIndex[2] > CubeArrayLastIndex[2] && Dquadrant[2] > 0))) { CubicSpace cubic = GetCubicSpace(currentIndex); if (cubic != null) Ray.Add(cubic); int Dir = 0; double minV = v1; if ( minV > v2) Dir = 1; minV = v2; if (minV > v3) Dir = 2; minV = v3; currentIndex[Dir] += Dquadrant[Dir]; if (Dir == 0) v1 = Dquadrant[0] != 0 ?Rate1 * Abs(currentIndex[0] + Dquadrant[0] - ssP[0]) : double.PositiveInfinity; else if (Dir == 1) v2 = Dquadrant[1] != 0 ?Rate2 * Abs(currentIndex[1] + Dquadrant[1] - ssP[1]) : double.PositiveInfinity; else if (Dir == 2) v3 = Dquadrant[2] != 0 ? Rate3 * Abs(currentIndex[2] + Dquadrant[2] - ssP[2]) : double.PositiveInfinity; } } #endregion return Ray; }
public HashSet <CubicSpace> GetRay(PointD position, Vector direction) { PointD ssP = GetStanderCoordinate(position); Vector ssD = Normalize(GetStanderVector(direction)); int[] Dquadrant = Sign(direction.ToArray()); int[] startIndex = GetCubicSpaceIndex(position); int[] currentIndex = GetCubicSpaceIndex(position); HashSet <CubicSpace> Ray = new HashSet <CubicSpace>(); #region XYZ射線距離計算 { double Rate1 = Dot(ssD, new Vector(1, 0, 0)); ////////////////// double Rate2 = Dot(ssD, new Vector(0, 1, 0)); ////////////////// double Rate3 = Dot(ssD, new Vector(0, 0, 1)); ////////////////// double v1 = Dquadrant[0] != 0 ? Rate1 * Abs(currentIndex[0] + Dquadrant[0] - ssP[0]) : double.PositiveInfinity; double v2 = Dquadrant[1] != 0 ? Rate2 * Abs(currentIndex[1] + Dquadrant[1] - ssP[1]) : double.PositiveInfinity; double v3 = Dquadrant[2] != 0 ? Rate3 * Abs(currentIndex[2] + Dquadrant[2] - ssP[2]) : double.PositiveInfinity; while (!((currentIndex[0] < 0 && Dquadrant[0] < 0) || (currentIndex[0] > CubeArrayLastIndex[0] && Dquadrant[0] > 0)) && !((currentIndex[1] < 0 && Dquadrant[1] < 0) || (currentIndex[1] > CubeArrayLastIndex[1] && Dquadrant[1] > 0)) && !((currentIndex[2] < 0 && Dquadrant[2] < 0) || (currentIndex[2] > CubeArrayLastIndex[2] && Dquadrant[2] > 0))) { CubicSpace cubic = GetCubicSpace(currentIndex); if (cubic != null) { Ray.Add(cubic); } int Dir = 0; double minV = v1; if (minV > v2) { Dir = 1; } minV = v2; if (minV > v3) { Dir = 2; } minV = v3; currentIndex[Dir] += Dquadrant[Dir]; if (Dir == 0) { v1 = Dquadrant[0] != 0 ?Rate1 * Abs(currentIndex[0] + Dquadrant[0] - ssP[0]) : double.PositiveInfinity; } else if (Dir == 1) { v2 = Dquadrant[1] != 0 ?Rate2 * Abs(currentIndex[1] + Dquadrant[1] - ssP[1]) : double.PositiveInfinity; } else if (Dir == 2) { v3 = Dquadrant[2] != 0 ? Rate3 * Abs(currentIndex[2] + Dquadrant[2] - ssP[2]) : double.PositiveInfinity; } } } #endregion return(Ray); }