public List<GameObject> GetBody(DVector3 Pos, double Radius)
{
TL = null;
TL = new List<GameObject>();
DVector3 AA_2, BB_2;
AA_2.X = Pos.X + Radius;
AA_2.Y = Pos.Y + Radius;
AA_2.Z = Pos.Z + Radius;
BB_2.X = Pos.X - Radius;
BB_2.Y = Pos.Y - Radius;
BB_2.Z = Pos.Z - Radius;
int t = TestTwoAABB(AA_2, BB_2, Root.AA, Root.BB);
if (t == 1)
{
TestIn(false, Root, Pos, Radius, AA_2, BB_2);
}
if (t == 2)
{
TestIn(true, Root, Pos, Radius, AA_2, BB_2);
}
//TestIn(false, Root, Pos, Radius);
return TL;
}
public KDtree(int MaxNestingLevel, List<GameObject> GameObjects)
{
this.MaxNestingLevel = MaxNestingLevel;
DVector3 AA = new DVector3();
DVector3 BB = new DVector3();
for (int i = 0; i < GameObjects.Count; i++)
{
GameObject GO = GameObjects[i];
if (GO != null)
{
if (AA.X < GO.AABBMax.X) AA.X = GO.AABBMax.X;
if (AA.Y < GO.AABBMax.Y) AA.Y = GO.AABBMax.Y;
if (AA.Z < GO.AABBMax.Z) AA.Z = GO.AABBMax.Z;
if (BB.X > GO.AABBMin.X) BB.X = GO.AABBMin.X;
if (BB.Y > GO.AABBMin.Y) BB.Y = GO.AABBMin.Y;
if (BB.Z > GO.AABBMin.Z) BB.Z = GO.AABBMin.Z;
}
}
DVector3 TAA = AA + (AA - BB) * 0.1d;
DVector3 TBB = BB + (BB - AA) * 0.1d;
Root = new Node(null, TAA, TBB, MaxNestingLevel);
for (int t = 0; t < GameObjects.Count; t++)
{
if (GameObjects[t] != null)
Add(GameObjects[t], Root);
}
}
public static double AngleTwoVec(DVector3 vec1, DVector3 vec2)
{
vec1.Normalize();
vec2.Normalize();
//return Math.Acos(((vec1.X * vec2.X + vec1.Y * vec2.Y + vec1.Z * vec2.Z) / (Math.Sqrt(vec1.X * vec1.X + vec1.Y * vec1.Y + vec1.Z * vec1.Z) * Math.Sqrt(vec2.X * vec2.X + vec2.Y * vec2.Y + vec2.Z * vec2.Z))));
double t4 = (vec1.X * vec2.X + vec1.Y * vec2.Y + vec1.Z * vec2.Z) / (Math.Sqrt(vec1.X * vec1.X + vec1.Y * vec1.Y + vec1.Z * vec1.Z)) * (Math.Sqrt(vec2.X * vec2.X + vec2.Y * vec2.Y + vec2.Z * vec2.Z));
t4 = Math.Min(1, t4);
t4 = Math.Max(-1, t4);
return Math.Acos(t4);
}
/// <summary>
/// Угол между векторами в плоскости X
/// </summary>
/// <param name="vec1">Вектор1</param>
/// <param name="vec2">Вектор2</param>
/// <returns>Радианы</returns>
public static float AngleTwoVecX(DVector3 vec_1, DVector3 vec_2)
{
DVector3 vec1_t = vec_1;
DVector3 vec2_t = vec_2;
vec1_t.Normalize();
vec2_t.Normalize();
float t = 1;
if ((vec1_t.X * vec2_t.Z - vec2_t.X * vec1_t.Z) < 0) t = -1;
vec1_t.Y = 1f;
vec2_t.Y = 1f;
return t * (float)Math.Acos(((vec1_t.X * vec2_t.X + vec1_t.Y * vec2_t.Y + vec1_t.Z * vec2_t.Z) / (Math.Sqrt(vec1_t.X * vec1_t.X + vec1_t.Y * vec1_t.Y + vec1_t.Z * vec1_t.Z) * Math.Sqrt(vec2_t.X * vec2_t.X + vec2_t.Y * vec2_t.Y + vec2_t.Z * vec2_t.Z))));
}
public static void CartesianToSpherical(DVector3 cart, out double lon, out double lat)
{
var radius = cart.Length();
if (radius == 0.0) {
lon = 0;
lat = 0;
return;
}
lon = Math.Atan2(cart.X, cart.Z);
lat = Math.Asin(cart.Y / radius);
}
public GameObject(string MeshName)
{
Position = new DVector3();
MMC = ContentManager.LoadMultiMesh(MeshName);
LocalAABBMin = Conversion.ToDoubleVector(MMC.LocalAABBMin);
LocalAABBMax = Conversion.ToDoubleVector(MMC.LocalAABBMax);
Transformation = DMatrix.CreateScale(1d)*DMatrix.CreateTranslation(Position) * DMatrix.CreateFromYawPitchRoll(0, 0, 0);
RecalcAABB();
InitObj();
Lens = ContentManager.LoadTexture2D("Content/Textures/Lens");
}
public static bool LineIntersection(DVector3 lineOrigin, DVector3 lineEnd, double radius, out DVector3[] intersectionPoints)
{
intersectionPoints = new DVector3[0];
double sphereRadius = radius;
DVector3 sphereCenter = DVector3.Zero;
var lineDir = new DVector3(lineEnd.X - lineOrigin.X, lineEnd.Y - lineOrigin.Y, lineEnd.Z - lineOrigin.Z);
lineDir.Normalize();
DVector3 w = lineOrigin - sphereCenter;
double a = DVector3.Dot(lineDir, lineDir); // 1.0f;
double b = 2 * DVector3.Dot(lineDir, w);
double c = DVector3.Dot(w, w) - sphereRadius * sphereRadius;
double d = b * b - 4.0f * a * c;
if (d < 0) return false;
if (d == 0.0) {
double x1 = (-b - Math.Sqrt(d)) / (2.0 * a);
intersectionPoints = new DVector3[1];
intersectionPoints[0] = lineOrigin + lineDir * x1;
return true;
}
if (d > 0.0f) {
double sqrt = Math.Sqrt(d);
double x1 = (-b - sqrt) / (2.0 * a);
double x2 = (-b + sqrt) / (2.0 * a);
intersectionPoints = new DVector3[2];
intersectionPoints[0] = lineOrigin + lineDir * x1;
intersectionPoints[1] = lineOrigin + lineDir * x2;
return true;
}
return false;
}
public void MoveFreeSurfaceCamera(DVector3 direction)
{
velocityDirection = direction;
}
public void Update(GameTime gameTime)
{
UpdateProjectionMatrix();
CameraPosition = DVector3.Transform(new DVector3(0, 0, CameraDistance), Rotation);
ViewMatrix = DMatrix.LookAtRH(CameraPosition, DVector3.Zero, DVector3.UnitY);
ViewMatrixWithTranslation = ViewMatrix;
ViewMatrix.TranslationVector = DVector3.Zero;
FinalCamPosition = CameraPosition;
if (CameraState == CameraStates.ViewToPoint) {
var mat = CalculateBasisOnSurface();
DVector3 lookAtPoint = mat.Up * EarthRadius;
double length = CameraDistance - EarthRadius;
var quat = DQuaternion.RotationAxis(DVector3.UnitY, ViewToPointYaw) * DQuaternion.RotationAxis(DVector3.UnitX, ViewToPointPitch);
var qRot = DMatrix.RotationQuaternion(quat);
var matrix = qRot * mat;
var pointOffset = DVector3.Transform(new DVector3(0, 0, length), matrix);
var camPoint = new DVector3(pointOffset.X, pointOffset.Y, pointOffset.Z) + lookAtPoint;
FinalCamPosition = camPoint;
ViewMatrix = DMatrix.LookAtRH(camPoint, lookAtPoint, mat.Up);
ViewMatrixWithTranslation = ViewMatrix;
ViewMatrix.TranslationVector = DVector3.Zero;
} else if (CameraState == CameraStates.FreeSurface) {
var mat = CalculateBasisOnSurface();
#region Input
// Update surface camera yaw and pitch
//if (input.IsKeyDown(Keys.RightButton)) {
// FreeSurfaceYaw += input.RelativeMouseOffset.X*0.0003;
// FreeSurfacePitch -= input.RelativeMouseOffset.Y*0.0003;
//
// input.IsMouseCentered = false;
// input.IsMouseHidden = true;
//}
//else {
// input.IsMouseCentered = false;
// input.IsMouseHidden = false;
//}
//if (Game.Keyboard.IsKeyDown(Input.Keys.Left)) FreeSurfaceYaw -= gameTime.ElapsedSec * 0.7;
//if (Game.Keyboard.IsKeyDown(Input.Keys.Right)) FreeSurfaceYaw += gameTime.ElapsedSec * 0.7;
//if (Game.Keyboard.IsKeyDown(Input.Keys.Up)) FreeSurfacePitch -= gameTime.ElapsedSec * 0.7;
//if (Game.Keyboard.IsKeyDown(Input.Keys.Down)) FreeSurfacePitch += gameTime.ElapsedSec * 0.7;
//FreeSurfaceYaw = DMathUtil.Clamp(FreeSurfaceYaw, -DMathUtil.PiOverTwo, DMathUtil.PiOverTwo);
if (FreeSurfaceYaw > DMathUtil.TwoPi) FreeSurfaceYaw -= DMathUtil.TwoPi;
if (FreeSurfaceYaw < -DMathUtil.TwoPi) FreeSurfaceYaw += DMathUtil.TwoPi;
// Calculate free cam rotation matrix
if (!freezeFreeCamRotation)
freeSurfaceRotation = DQuaternion.RotationAxis(DVector3.UnitY, FreeSurfaceYaw) * DQuaternion.RotationAxis(DVector3.UnitX, FreeSurfacePitch);
var quat = freeSurfaceRotation;
var qRot = DMatrix.RotationQuaternion(quat);
var matrix = qRot * mat;
var velDir = matrix.Forward * velocityDirection.X + mat.Up * velocityDirection.Y + matrix.Right * velocityDirection.Z;
if (velDir.Length() != 0) {
velDir.Normalize();
}
#endregion
double fac = ((CameraDistance - EarthRadius) - Parameters.MinDistVelocityThreshold) / (Parameters.MaxDistVelocityThreshold - Parameters.MinDistVelocityThreshold);
fac = DMathUtil.Clamp(fac, 0.0, 1.0);
FreeSurfaceVelocityMagnitude = DMathUtil.Lerp(Parameters.MinVelocityFreeSurfCam, Parameters.MaxVelocityFreeSurfCam, fac);
// Update camera position
FinalCamPosition = FreeSurfacePosition = FreeSurfacePosition + velDir * FreeSurfaceVelocityMagnitude * gameTime.ElapsedSec;
CameraPosition = FinalCamPosition;
velocityDirection = DVector3.Zero;
//Calculate view matrix
ViewMatrix = DMatrix.LookAtRH(FinalCamPosition, FinalCamPosition + matrix.Forward, matrix.Up);
ViewMatrixWithTranslation = ViewMatrix;
ViewMatrix.TranslationVector = DVector3.Zero;
// Calculate new yaw and pitch
CameraDistance = CameraPosition.Length();
var newLonLat = GetCameraLonLat();
Yaw = newLonLat.X;
Pitch = -newLonLat.Y;
}
ViewMatrixFloat = DMatrix.ToFloatMatrix(ViewMatrix);
ProjMatrixFloat = DMatrix.ToFloatMatrix(ProjMatrix);
//var viewDir = CameraPosition / CameraPosition.Length();
}
private void TestIn(bool Pol, Node N, DVector3 Pos, double Radius, DVector3 AA, DVector3 BB)
{
if (!Pol)
{
for (int tx = 0; tx < N.ListBSSO.Count; tx++ )
{
if (N.ListBSSO[tx] != null)
{
int t = TestTwoSphere(Pos, Radius, N.ListBSSO[tx].Position,
Math.Max((N.ListBSSO[tx].Position - N.ListBSSO[tx].AABBMax).Length(), (N.ListBSSO[tx].Position - N.ListBSSO[tx].AABBMin).Length()));
if (t == 1 || t == 2) TL.Add(N.ListBSSO[tx]);
}
}
}
else
{
TL.AddRange(N.ListBSSO);
}
if (N.Children.Count > 0)
{
int tt;
bool B = false;
Node N_ = N.Children[0];
tt = TestTwoAABB(AA, BB, N_.AA, N_.BB);
if (tt == 1)
{
B = false;
TestIn(false, N_, Pos, Radius, AA, BB);
}
if (tt == 2)
{
B = true;
TestIn(true, N_, Pos, Radius, AA, BB);
}
N_ = N.Children[1];
tt = TestTwoAABB(AA, BB, N_.AA, N_.BB);
if (tt == 1)
{
TestIn(false, N_, Pos, Radius, AA, BB);
}
if (tt == 2)
{
TestIn(true, N_, Pos, Radius, AA, BB);
}
}
}
public void ToggleFreeSurfaceCamera()
{
if (CameraState == CameraStates.FreeSurface) {
//CameraState = CameraStates.TopDown;
} else {
CameraState = CameraStates.FreeSurface;
FreeSurfacePosition = CameraPosition;
}
}
void SetState(SurfaceCameraState state)
{
CameraPosition = FreeSurfacePosition = state.FreeSurfacePosition;
CameraDistance = CameraPosition.Length();
FreeSurfaceYaw = state.FreeSurfaceYaw;
FreeSurfacePitch = state.FreeSurfacePitch;
freeSurfaceRotation = state.FreeRotation;
var newLonLat = GetCameraLonLat();
Yaw = newLonLat.X;
Pitch = -newLonLat.Y;
}
public static extern int TangoSupport_fitPlaneModelNearPointMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out DVector3 intersectionPoint,
[Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 4)] double[] planeModel);
public List<GameObject> GetBodyInCamera(DVector3 Position, BoundingFrustum LocalCameraBoundingFrustum)
{
//TL = null;
TL = new List<GameObject>();
this.Position = Position;
TestInCam(false, Root, LocalCameraBoundingFrustum);
//return TL;
List<GameObject> T = new List<GameObject> { };
for (int t = 0; t < TL.Count; t++ )
{
if (TestFrustum(new BoundingBox(Conversion.ToVector3(TL[t].AABBMin - Position), Conversion.ToVector3(TL[t].AABBMax - Position)), LocalCameraBoundingFrustum) > 0)
{
T.Add(TL[t]);
}
}
return T;
}
public DVector2 GetCameraLonLat()
{
var nearPoint = new DVector3((CameraPosition.X / CameraDistance), (CameraPosition.Y / CameraDistance), (CameraPosition.Z / CameraDistance));
var farPoint = new DVector3(0, 0, 0);
DVector3[] res;
DVector2 ret = DVector2.Zero;
if (GeoHelper.LineIntersection(nearPoint, farPoint, 1.0, out res)) {
if (res.Length > 0) {
GeoHelper.CartesianToSpherical(res[0], out ret.X, out ret.Y);
}
}
return ret;
}
public Node(Node parent, DVector3 AA, DVector3 BB, int nestingLevel)
{
Parent = parent;
this.AA = AA;
this.BB = BB;
NestingLevel = nestingLevel;
ListBSSO = new List<GameObject>();
Children = new List<Node>();
}
public override List<Gis.SelectedItem> Select(DVector3 nearPoint, DVector3 farPoint)
{
throw new NotImplementedException();
}
private int TestTwoSphere(DVector3 Pos, double Radius, DVector3 Pos_2, double Radius_2)
{
double Dist = (Pos - Pos_2).Length();
double Sum = Radius + Radius_2;
if (Dist > Sum) return 0;
if (Dist < Radius_2) return 2;
return 1;
}
private int TestTwoAABB(DVector3 AA, DVector3 BB, DVector3 AA_2, DVector3 BB_2)
{
if (AA.X < BB_2.X || BB.X > AA_2.X) return 0;
if (AA.Y < BB_2.Y || BB.Y > AA_2.Y) return 0;
if (AA.Z < BB_2.Z || BB.Z > AA_2.Z) return 0;
if (AA.X >= AA_2.X && BB.X <= BB_2.X && AA.Y >= AA_2.Y && BB.Y <= BB_2.Y && AA.Z >= AA_2.Z && BB.Z <= BB_2.Z) return 2;
return 1;
//0 - нет
//1 - частично
//2 - полностью
}
public List<GameObject> GetRay(DVector3 Start, DVector3 Stop)
{
List<GameObject> TLInd = new List<GameObject> { };
TestIntercept(TLInd, Root, Start, Stop - Start);
return TLInd;
}
public void GetRayFromScreenPoint(float x, float y, out DVector3 near, out DVector3 far)
{
var w = Viewport.Width;
var h = Viewport.Height;
var nearPoint = new DVector3(x, y, Parameters.FrustumZNear);
var farPoint = new DVector3(x, y, Parameters.FrustumZFar);
var vm = ViewMatrixWithTranslation;
var mVP = vm * ProjMatrix;
near = DVector3.Unproject(nearPoint, 0, 0, w, h, Parameters.FrustumZNear, Parameters.FrustumZFar, mVP);
far = DVector3.Unproject(farPoint, 0, 0, w, h, Parameters.FrustumZNear, Parameters.FrustumZFar, mVP);
}
public override List<Gis.SelectedItem> Select(DVector3 nearPoint, DVector3 farPoint)
{
DVector3[] rayHitPoints;
var ret = new List<Gis.SelectedItem>();
if (!GeoHelper.LineIntersection(nearPoint, farPoint, GeoHelper.EarthRadius, out rayHitPoints)) return ret;
var rayLonLatRad = GeoHelper.CartesianToSpherical(rayHitPoints[0]);
//var OneGradusLengthKmInv = 1.0 / (Math.Cos(rayLonLatRad.Y)*GeoHelper.EarthOneDegreeLengthOnEquatorMeters/1000.0);
for (int i = 0; i < PointsCountToDraw; i++) {
int ind = PointsDrawOffset + i;
var point = PointsCpu[ind];
var size = point.Tex0.Z * 0.5;
var pointLonLat = new DVector2(point.Lon, point.Lat);
var dist = GeoHelper.DistanceBetweenTwoPoints(pointLonLat, rayLonLatRad);
if (dist <= size) {
ret.Add(new SelectedItem {
Distance = dist,
PointIndex = ind
});
}
}
return ret;
}
public Vector2 CartesianToScreen(DVector3 cartPos)
{
var p = DVector3.Project(cartPos, Viewport.X, Viewport.Y, Viewport.Width, Viewport.Height, Parameters.FrustumZNear, Parameters.FrustumZFar, ViewMatrixWithTranslation * ProjMatrix);
return new Vector2((float)p.X, (float)p.Y);
}
public override List<Gis.SelectedItem> Select(DVector3 nearPoint, DVector3 farPoint)
{
return null;
}
public void PlayAnimation(GameTime gameTime)
{
if(cameraAnimTrackStates == null) return;
freezeFreeCamRotation = true;
var state = cameraAnimTrackStates[curStateInd];
curTime += 0.016f; //gameTime.ElapsedSec;
if (curTime < state.WaitTime || curStateInd >= cameraAnimTrackStates.Count - 1) {
SetState(state);
if (curStateInd >= cameraAnimTrackStates.Count - 1)
StopAnimation();
return;
}
float time = curTime - state.WaitTime;
float amount = time/state.TransitionTime;
float factor = MathUtil.SmoothStep(amount);
factor = MathUtil.Clamp(factor, 0.0f, 1.0f);
var nextState = cameraAnimTrackStates[curStateInd+1];
var curPos = DVector3.Lerp(state.FreeSurfacePosition, nextState.FreeSurfacePosition, factor);
var curFreeRot = DQuaternion.Slerp(state.FreeRotation, nextState.FreeRotation, factor);
freeSurfaceRotation = curFreeRot;
CameraPosition = FreeSurfacePosition = curPos;
var newLonLat = GetCameraLonLat();
Yaw = newLonLat.X;
Pitch = -newLonLat.Y;
if (curTime > state.WaitTime + state.TransitionTime) {
curStateInd++;
curTime = 0;
}
}
public static DVector2 CartesianToSpherical(DVector3 cart)
{
double lon, lat;
CartesianToSpherical(cart, out lon, out lat);
return new DVector2(lon, lat);
}
/// <summary>
/// Fits a plane to a point cloud near a user-specified location. This
/// occurs in two passes. First, all points in cloud within
/// <c>maxPixelDistance</c> to <c>uvCoordinates</c> after projection are kept. Then a
/// plane is fit to the subset cloud using RANSAC. After the initial fit
/// all inliers from the original cloud are used to refine the plane
/// model.
/// </summary>
/// <returns>
/// Common.ErrorType.TANGO_SUCCESS on success,
/// Common.ErrorType.TANGO_INVALID on invalid input, and
/// Common.ErrorType.TANGO_ERROR on failure.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least three points.
/// </param>
/// <param name="pointCount">
/// The number of points to read from the point cloud.
/// </param>
/// <param name="timestamp">The timestamp of the point cloud.</param>
/// <param name="cameraIntrinsics">
/// The camera intrinsics for the color camera. Cannot be null.
/// </param>
/// <param name="matrix">
/// Transformation matrix of the color camera with respect to the Unity
/// World frame.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// between (0.0, 0.0) and (1.0, 1.0).
/// </param>
/// <param name="intersectionPoint">
/// The output point in depth camera coordinates that the user selected.
/// </param>
/// <param name="plane">The plane fit.</param>
public static int FitPlaneModelNearClick(
Vector3[] pointCloud, int pointCount, double timestamp,
TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix,
Vector2 uvCoordinates, out Vector3 intersectionPoint,
out Plane plane)
{
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud,
GCHandleType.Pinned);
TangoXYZij pointCloudXyzIj = new TangoXYZij();
pointCloudXyzIj.timestamp = timestamp;
pointCloudXyzIj.xyz_count = pointCount;
pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();
DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);
// Unity has Y pointing screen up; Tango camera has Y pointing
// screen down.
Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x,
1.0f - uvCoordinates.y);
DVector3 doubleIntersectionPoint = new DVector3();
double[] planeArray = new double[4];
int returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPointMatrixTransform(
pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix,
ref uvCoordinatesTango,
out doubleIntersectionPoint, planeArray);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f);
plane = new Plane(new Vector3(0.0f, 0.0f, 0.0f), 0.0f);
}
else
{
intersectionPoint = doubleIntersectionPoint.ToVector3();
Vector3 normal = new Vector3((float)planeArray[0],
(float)planeArray[1],
(float)planeArray[2]);
float distance = (float)planeArray[3] / normal.magnitude;
plane = new Plane(normal, distance);
}
pointCloudHandle.Free();
return returnValue;
}
/// <summary>
///
/// </summary>
/// <param name="lon"></param>
/// <param name="lat"></param>
/// <param name="radius"></param>
/// <param name="cart"></param>
public static void SphericalToCartesian(double lon, double lat, float radius, out DVector3 cart)
{
cart = DVector3.Zero;
double x, y, z;
SphericalToCartesian(lon, lat, radius, out x, out y, out z);
cart.X = x;
cart.Y = y;
cart.Z = z;
}
public static int TangoSupport_fitPlaneModelNearPointMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out DVector3 intersectionPoint, double[] planeModel)
{
intersectionPoint = new DVector3();
return Common.ErrorType.TANGO_SUCCESS;
}
private int SphereInAABB(DVector3 AA, DVector3 BB, DVector3 Pos, float Radius)
{
DVector3 AA_2, BB_2;
AA_2.X = Pos.X + Radius;
AA_2.Y = Pos.Y + Radius;
AA_2.Z = Pos.Z + Radius;
BB_2.X = Pos.X - Radius;
BB_2.Y = Pos.Y - Radius;
BB_2.Z = Pos.Z - Radius;
return TestTwoAABB(AA, BB, AA_2, BB_2);
}