/// <summary>
/// Initializes a new instance of the <see cref= "T:WorldWind.BoundingSphere"/> class
/// from a set of lat/lon values (degrees)
/// </summary>
public BoundingSphere( float south, float north, float west, float east, float radius1, float radius2)
{
// Compute the points in world coordinates
const int CornerCount = 8;
Vector3[] corners = new Vector3[CornerCount];
float scale = radius2 / radius1;
corners[0] = MathEngine.SphericalToCartesian(south, west, radius1);
corners[1] = Vector3.Scale(corners[0], scale);
corners[2] = MathEngine.SphericalToCartesian(south, east, radius1);
corners[3] = Vector3.Scale(corners[2], scale);
corners[4] = MathEngine.SphericalToCartesian(north, west, radius1);
corners[5] = Vector3.Scale(corners[4], scale);
corners[6] = MathEngine.SphericalToCartesian(north, east, radius1);
corners[7] = Vector3.Scale(corners[6], scale);
//Find the center. In this case, we'll simply average the coordinates.
foreach(Vector3 v in corners)
Center += v;
Center.Scale(1/CornerCount);
//Loop through the coordinates and find the maximum distance from the center. This is the radius.
foreach(Vector3 v in corners)
{
float distSq = Vector3.Subtract(v,Center).LengthSq();
if (distSq > Radius)
Radius = distSq;
}
Radius = (float)Math.Sqrt(Radius);
}
public Vector3 CalculateCenter()
{
Vector3 res = new Vector3();
foreach(Vector3 corner in corners)
{
res += corner;
}
res.Scale(1.0f / corners.Length);
return res;
}
/// <summary>
/// Return stats about the creature being edited, for display on the VDU
/// </summary>
public void Stats(out float radius, out float mass, out float bouyancy, out float resistance,
out Vector3 balance, out string name)
{
// Ensure the data is accurate after recent edits
Refresh(); // Make sure that the trees and lists are up-to-date
CalculateCG(); // Temporarily recalculate the CofG cell
RecursiveUpdateFrames(rootCell); // calculate cell positions
ComputeBoundingSphere(); // update radius and centre
radius = this.AbsSphere.Radius; // org radius
name = genome.Name; // current name
mass = 0;
bouyancy = 0;
resistance = 0;
foreach (Cell c in partList) // sum physical properties
{
mass += c.Physiology.Mass * scale;
bouyancy += c.Physiology.Buoyancy * scale;
resistance += c.Physiology.Resistance * scale;
}
balance = CGCell.AbsSphere.Centre - AbsSphere.Centre; // distance of CG from geometric centre (on each WORLD axis)
balance.Scale(1.0f / radius); // as a fraction of creature's size
// Clean up
CGCell = rootCell; // Set the CofG back to the root cell
}
/// <summary>
/// Virtual method implemented by all Renderable objects.
/// The given cell is in collision with our bounding sphere. Test to see if it actually collides with
/// one of my parts. If so, return a Vector describing the force we exert on the offending cell
/// (since we're an organism we'll receive a share of the force too, unlike scenery)
/// </summary>
/// <param name="cell">The cell that may have collided with us</param>
/// <returns> Any force vector acting on the cell </returns>
public override Vector3 CollisionTest(Cell otherCell)
{
Vector3 bounce = new Vector3();
// Run through our own cells looking for a collision
foreach (Cell ourCell in partList)
{
// Test more deeply by looking at the cell spheres then the
// bounding boxes of each mesh in the cells
if (Cell.CollisionTest(ourCell, otherCell) == true)
{
// The bounce direction is a vector in the direction of our centre towards theirs
// (in other words, the two cells are treated as spheres and therefore bounce back away from
// their centres).
bounce = otherCell.AbsSphere.Centre - ourCell.AbsSphere.Centre;
bounce.Normalize();
// The length of the force vector is proportional to the objects' closing speed
Vector3 ourMvt = ourCell.Location - ourCell.OldLocation; // how much we will move next frame
Vector3 hisMvt = otherCell.Location - otherCell.OldLocation; // how much he will move next frame
float speed = Vector3.Length(ourMvt + hisMvt); // combine to get closing movement
bounce.Scale(2.0f + speed * 10.0f); // arbitrary scaling factor
// Also, a force acts on OUR cell, in the reverse direction.
// Distribute the two forces inversely proportional to mass
Vector3 ourBounce = -bounce; // effect on us is the inverse of our effect on him
float mass = otherCell.Owner.TotalMass; // get the masses of the two ORGANISMS (not cells)
float ourMass = ourCell.Owner.TotalMass;
bounce.Scale(ourMass / (ourMass + mass)); // distribute the forces proportionately
ourBounce.Scale(mass / (ourMass + mass));
ourCell.propulsionForce += ourBounce; // apply our share as propulsion
// Once we've found a collision, we needn't look at any more of our cells
return bounce;
}
}
return bounce;
}
/// <summary>
/// Check to see if we're touching the water surface, and respond appropriately
/// </summary>
private void CheckForSurfaceCollision()
{
// We're quite safe if bounding sphere is below water level
if ((AbsSphere.Centre.Y + AbsSphere.Radius) < Water.WATERLEVEL)
return;
// Now check each cell in turn
foreach (Cell cell in partList)
{
// If the dist from centre to surface is smaller than the radius our spheres are in contact
float aboveSurface = cell.AbsSphere.Centre.Y + cell.AbsSphere.Radius - Water.WATERLEVEL;
if (aboveSurface >= 0)
{
// TODO: either look deeper at the mesh OBBs, or maybe take account of the density of the object
// to derive an approximate flotation height
// Our bounce vector is simply a down vector, proportional to our height above surface
Vector3 bounce = new Vector3(0,-1,0);
bounce.Scale(aboveSurface
* 0.5f); // arbitrary scaling factor
// Add the bounce as a force acting on our cell
cell.propulsionForce += bounce;
//Engine.DebugHUD.WriteLine("Surface!");
}
}
}
/// <summary>
/// Find the intersection of a ray with the terrain.
/// </summary>
/// <param name="p1">Cartesian coordinate of starting point</param>
/// <param name="p2">Cartesian coordinate of end point</param>
/// <param name="samplingPrecision">Sample length in meter</param>
/// <param name="resultPrecision">Final sampling length in meter</param>
/// <param name="latitude">Out : intersection latitude</param>
/// <param name="longitude">Out : intersection longitude</param>
/// <param name="world">Current world</param>
/// <returns>NaN if no intersection found</returns>
internal static void RayIntersectionWithTerrain(
Point3d p1,
Point3d p2,
double samplingPrecision,
double resultPrecision,
out Angle latitude,
out Angle longitude,
World world)
{
// Check for sphere intersection first
// Note : checks for world radius + highest possible elevation
float vertEx = World.Settings.VerticalExaggeration;
double maxRadius = world.EquatorialRadius + 9000 * vertEx; // Max altitude for earth - should be dependant on world
double a = (p2.X - p1.X) * (p2.X - p1.X) + (p2.Y - p1.Y) * (p2.Y - p1.Y) + (p2.Z - p1.Z) * (p2.Z - p1.Z);
double b = 2.0 * ((p2.X - p1.X) * (p1.X) + (p2.Y - p1.Y) * (p1.Y) + (p2.Z - p1.Z) * (p1.Z));
double c = p1.X * p1.X + p1.Y * p1.Y + p1.Z * p1.Z - maxRadius * maxRadius;
double discriminant = b * b - 4 * a * c;
if (discriminant <= 0)
{
// No intersection with sphere
latitude = Angle.NaN;
longitude = Angle.NaN;
return;
}
// Factor to intersection
// Note : if t1 > 0 intersection is forward, < 0 is behind us
double t1 = ((-1.0) * b - Math.Sqrt(discriminant)) / (2 * a);
Point3d p1LatLon = MathEngine.CartesianToSpherical(p1.X, p1.Y, p1.Z);
if (t1 > 0 && p1LatLon.X > maxRadius)
{
// Looking from above max altitude : move p1 forward to intersection with max alt sphere
p1 = new Point3d(p1.X + t1 * (p2.X - p1.X), p1.Y + t1 * (p2.Y - p1.Y), p1.Z + t1 * (p2.Z - p1.Z));
}
// Ray sample
Vector3 sample = new Vector3((float)(p2.X - p1.X), (float)(p2.Y - p1.Y), (float)(p2.Z - p1.Z));
double maxLength = sample.Length(); // Max length for ray tracing
double sampleLength = samplingPrecision;// Sampling steps length
sample.Normalize();
sample.Scale((float)sampleLength);
// Casting
Point3d ray = p1;
double rayLength = 0;
while (rayLength < maxLength)
{
Point3d rayLatLon = MathEngine.CartesianToSpherical(ray.X, ray.Y, ray.Z);
// Altitude at ray position
double rayAlt = rayLatLon.X - world.EquatorialRadius;
// Altitude at terrain position - from cached data (no download)
double terrainAlt = world.TerrainAccessor.GetCachedElevationAt(MathEngine.RadiansToDegrees(rayLatLon.Y), MathEngine.RadiansToDegrees(rayLatLon.Z)); // best loaded data
if (double.IsNaN(terrainAlt)) terrainAlt = 0;
terrainAlt *= vertEx;
if (terrainAlt > rayAlt)
{
// Intersection found
if (sampleLength > resultPrecision)
{
// Go back one step
ray.X -= sample.X;
ray.Y -= sample.Y;
ray.Z -= sample.Z;
rayLength -= sampleLength;
// and refine sampling
sampleLength /= 10;
sample.Normalize();
sample.Scale((float)sampleLength);
}
else
{
// return location
latitude = Angle.FromRadians(rayLatLon.Y);
longitude = Angle.FromRadians(rayLatLon.Z);
return;
}
}
// Move forward
ray.X += sample.X;
ray.Y += sample.Y;
ray.Z += sample.Z;
rayLength += sampleLength;
}
// No intersection with terrain found
latitude = Angle.NaN;
longitude = Angle.NaN;
}
/// <summary>
/// NOT IMPLEMENTED
/// </summary>
/// <param name="normal"></param>
/// <param name="tangent"></param>
/// <exception cref="NotImplementedException"></exception>
public static void OrthoNormalize(ref Vector3 normal, ref Vector3 tangent)
{
normal.Normalize();
var proj = new Vector3();
proj.Scale(normal);
throw new NotImplementedException();
}
bool CalculateRectPlacement(DrawArgs drawArgs)
{
int labelLinePoint = FindAnchorPoint();
if(labelLinePoint < 0)
{
// Measure line is not visible
return false;
}
Vector3 referenceCenter = new Vector3(
(float)drawArgs.WorldCamera.ReferenceCenter.X,
(float)drawArgs.WorldCamera.ReferenceCenter.Y,
(float)drawArgs.WorldCamera.ReferenceCenter.Z
);
Angle displayAngle = CalcAngle(labelLinePoint, referenceCenter);
if( Angle.IsNaN(displayAngle) )
return false;
const int leg1Len = 30;
const int leg2Len = 5;
Vector3 screenAnchor = m_drawArgs.WorldCamera.Project(
new Vector3(
measureLine[labelLinePoint].X,
measureLine[labelLinePoint].Y,
measureLine[labelLinePoint].Z ) - referenceCenter);
float x1 = (float)(screenAnchor.X + Math.Cos(displayAngle.Radians)*leg1Len);
float y1 = (float)(screenAnchor.Y + Math.Sin(displayAngle.Radians)*leg1Len);
float x2 = x1;
float y2 = y1;
// Find direction of 2nd leg.
int quadrant = (int)((displayAngle.Radians)/(Math.PI/2));
switch (quadrant % 4)
{
case 0:
case 3:
x2 += leg2Len;
break;
case 1:
case 2:
x2 -= leg2Len;
break;
}
// Calculate label box position / size
if (World.Settings.MeasureMode == MeasureMode.Multi)
{
Distance = multiline.getLength();
//labelText = Distance>=10000 ?
// string.Format( "Total Distance: {0:f1}km", Distance/1000 ) :
// string.Format( "Total Distance: {0:f1}m", Distance );
labelText = "Total Distance: " + ConvertUnits.GetDisplayString(Distance);
}
else
{
//labelText = Distance>=10000 ?
// string.Format( "Distance: {0:f1}km", Distance/1000 ) :
// string.Format( "Distance: {0:f1}m", Distance );
labelText = "Distance: " + ConvertUnits.GetDisplayString(Distance);
}
labelText += string.Format("\nBearing: {0:f1}°", Azimuth.Degrees );
labelTextRect = m_drawArgs.defaultDrawingFont.MeasureString(null, labelText, DrawTextFormat.None, 0);
Rectangle tsize = labelTextRect;
const int xPad = 4;
const int yPad = 1;
tsize.Inflate( xPad, yPad );
labelTextRect.Offset(-tsize.Left, -tsize.Top);
tsize.Offset(-tsize.Left, -tsize.Top);
rectLineConnection[0].X = screenAnchor.X;
rectLineConnection[0].Y = screenAnchor.Y;
rectLineConnection[1].X = x1;
rectLineConnection[1].Y = y1;
rectLineConnection[2].X = x2;
rectLineConnection[2].Y = y2;
if(x2>x1)
{
labelTextRect.Offset((int)x2, 0);
tsize.Offset((int)x2, 0);
}
else
{
int xof = (int)(x2-tsize.Width);
labelTextRect.Offset(xof, 0);
tsize.Offset(xof, 0);
}
tsize.Offset(0, (int)(y2 - tsize.Height/2));
labelTextRect.Offset(0, (int)(y2 - tsize.Height/2));
rect[0].X = tsize.Left;
rect[0].Y = tsize.Top;
rect[1].X = rect[0].X;
rect[1].Y = tsize.Bottom;
rect[2].X = tsize.Right;
rect[2].Y = rect[0].Y;
rect[3].X = rect[2].X;
rect[3].Y = rect[1].Y;
rect[4].X = rect[0].X;
rect[4].Y = rect[1].Y;
rectFrame[0].X = tsize.Left;
rectFrame[0].Y = tsize.Top;
rectFrame[1].X = rectFrame[0].X;
rectFrame[1].Y = tsize.Bottom;
rectFrame[2].X = tsize.Right;
rectFrame[2].Y = rectFrame[1].Y;
rectFrame[3].X = rectFrame[2].X;
rectFrame[3].Y = rectFrame[0].Y;
rectFrame[4].X = rectFrame[0].X;
rectFrame[4].Y = rectFrame[0].Y;
// Cap at start of measure
Vector3 a = new Vector3(measureLine[0].X, measureLine[0].Y, measureLine[0].Z );
Vector3 b = new Vector3(measureLine[1].X, measureLine[1].Y, measureLine[1].Z );
Vector3 vCap = Vector3.Cross(a,b);
vCap.Normalize();
const int lineCapSize = 6;
vCap.Scale( (float)m_drawArgs.WorldCamera.Distance/750f*lineCapSize );
Vector3 worldXyzStart = new Vector3( measureLine[0].X, measureLine[0].Y, measureLine[0].Z );
Vector3 va = Vector3.Add( worldXyzStart, vCap );
Vector3 vb = Vector3.Add( worldXyzStart, -vCap );
startPoint[0].X = va.X;
startPoint[0].Y = va.Y;
startPoint[0].Z = va.Z;
startPoint[1].X = vb.X;
startPoint[1].Y = vb.Y;
startPoint[1].Z = vb.Z;
// Cap at end of measure
int last = measureLine.Length-1;
Vector3 worldXyzEnd = new Vector3(
measureLine[last].X,
measureLine[last].Y,
measureLine[last].Z );
int beforeLast = last-1;
vCap = new Vector3(
measureLine[beforeLast].X,
measureLine[beforeLast].Y,
measureLine[beforeLast].Z );
vCap.Subtract(worldXyzEnd);
vCap.Normalize();
vCap.Scale( (float)(m_drawArgs.WorldCamera.Distance/750f*lineCapSize) );
vb = va = Vector3.Add( worldXyzEnd , vCap );
const float arrowHeadAngle = 0.25f*(float)Math.PI;
va.TransformCoordinate( Matrix.RotationAxis( worldXyzEnd, (float)Math.PI+arrowHeadAngle ));
vb.TransformCoordinate( Matrix.RotationAxis( worldXyzEnd, arrowHeadAngle));
endPoint[0].X = va.X;
endPoint[0].Y = va.Y;
endPoint[0].Z = va.Z;
endPoint[1].X = vb.X;
endPoint[1].Y = vb.Y;
endPoint[1].Z = vb.Z;
Matrix rotate90 = Matrix.RotationAxis( worldXyzEnd, (float)Math.PI*0.5f );
va.TransformCoordinate( rotate90 );
vb.TransformCoordinate( rotate90 );
endPoint[2].X = va.X;
endPoint[2].Y = va.Y;
endPoint[2].Z = va.Z;
endPoint[3].X = vb.X;
endPoint[3].Y = vb.Y;
endPoint[3].Z = vb.Z;
return true;
}
public static void Test_Vector3_Generic_Scale_Against_Unity3D(float a, float b, float c, float d, float e, float f)
{
//arrange
Vector3<float> genericVec3One = new Vector3<float>(a, b, c);
Vector3<float> genericVec3Two = new Vector3<float>(d, e, f);
UnityEngine.Vector3 unityVec3One = new UnityEngine.Vector3(a, b, c);
UnityEngine.Vector3 unityVec3Two = new UnityEngine.Vector3(d, e, f);
//act
Vector3<float> genericScale = genericVec3One.Scale(genericVec3Two);
unityVec3One.Scale(unityVec3Two);
//assert
Assert.IsTrue(isVectorsEqual(unityVec3One, genericScale));
}