public void Vector3dAddsCorrectly()
{
var v = new Vector3d(2, 3, 4);
Assert.False(v == v + 0);
Assert.False(v == v.Add(0));
Assert.True(v.Equivalent(v + 0));
Assert.True(v.Equivalent(v.Add(0)));
Assert.True((v + v).Equivalent(new Vector3d(4, 6, 8)));
Assert.True(v.Add(v).Equivalent(new Vector3d(4, 6, 8)));
Assert.True((v + .1).Equivalent(new Vector3d(2.1, 3.1, 4.1)));
Assert.True(v.Add(.1).Equivalent(new Vector3d(2.1, 3.1, 4.1)));
}
public Sphere3d At(ref Vector3d position_world)
{
var copy = this;
Vector3d.Add(ref copy.Origin, ref position_world, out copy.Origin);
return(copy);
}
public void selectRandomDirection(AbstractEnvironmentType env, Libutility util, Vector3d preDir)
{
if (env.env_type == 1)
{
double randx = (util.getRandDouble() - 0.5) * 2;
double randy = (util.getRandDouble() - 0.5) * 2;
double randz = (util.getRandDouble() - 0.5) * 2;
Vector3d randDir = new Vector3d(randx, randy, randz);
randDir = Vector3d.Add(randDir, preDir);
Double leng = randDir.Length;
Double factor = _cur_speed / leng;
orientation = Vector3d.Multiply(factor, randDir);
return;
}
else if (env.env_type == 2)
{
double randx = (util.getRandDouble() - 0.5) * 2;
double randy = (util.getRandDouble() - 0.5) * 2;
Vector3d randDir = new Vector3d(randx, randy, 0);
randDir = Vector3d.Add(randDir, preDir);
randDir.Unitize();
uv_orientation = Vector3d.Multiply(_cur_speed, randDir);
orientation = env.getOrientationFromUv(Location, uv_orientation);
return;
}
}
public static Vector3d CalcSum(AgentType agent, IList <AgentType> agents, out int count)
{
Vector3d sum = new Vector3d();
count = 0;
foreach (AgentType other in agents)
{
double d = agent.Position3D.DistanceTo(other.Position3D);
//double d = Vector3d.Subtract(agent.Position, other.Position).Length;
//if we are not comparing the seeker to iteself and it is at least
//desired separation away:
if (d > 0)
{
Vector3d diff = Point3d.Subtract(agent.Position3D, other.Position3D);
diff.Unitize();
//Weight the magnitude by distance to other
diff = Vector3d.Divide(diff, d);
sum = Vector3d.Add(sum, diff);
//For an average, we need to keep track of how many boids
//are in our vision.
count++;
}
}
return(sum);
}
private Vector3d findAndUpdateNewAverageNormal()//send some of this out to smaller function?
{
Vector3d newNormal = new Vector3d(0.0, 0.0, 0.0);
double totalArea = AreaMassProperties.Compute(proxyAsMesh).Area;
for (int i = 0; i < assignedFaces.Count; i++)
{
Vector3f normal = partition.controller.rhinoMesh.FaceNormals[assignedFaces[i]];
Rhino.Geometry.Mesh mesh = new Rhino.Geometry.Mesh();
for (int j = 0; j < partition.controller.wingMesh.faces[assignedFaces[i]].faceVerts.Count; j++)
{
Vector3f position = partition.controller.wingMesh.faces[assignedFaces[i]].faceVerts[j].position;
mesh.Vertices.Add(position.X, position.Y, position.Z);
}
if (partition.controller.wingMesh.faces[assignedFaces[i]].faceVerts.Count == 3)
{
mesh.Faces.AddFace(0, 1, 2);
}
else if (partition.controller.wingMesh.faces[assignedFaces[i]].faceVerts.Count == 4)
{
mesh.Faces.AddFace(0, 1, 2, 3);
}
double area = AreaMassProperties.Compute(mesh).Area;
mesh.Normals.ComputeNormals();
Vector3d areaWeightedNormal = Vector3d.Multiply(area / totalArea, mesh.FaceNormals[0]);
newNormal = Vector3d.Add(newNormal, areaWeightedNormal);
}
return(Vector3d.Divide(newNormal, assignedFaces.Count));
}
public static void PerturbVectors(List <Vector3d> vectors)
{
for (int i = 0; i < vectors.Count; i++)
{
Vector3d p = vectors[i];
Vector3d perturb = new Vector3d();
if (i % 3 == 0)
{
perturb.X = 2; perturb.Y = 0; perturb.Z = 1;
}
else if (i % 3 == 1)
{
perturb.X = 0; perturb.Y = 2; perturb.Z = 2;
}
else
{
perturb.X = 1; perturb.Y = 3; perturb.Z = 4;
}
p = Vector3d.Add(p, perturb);
vectors[i] = p;
}
}
//Functions for Motion (i dont think anything is updating position at the moment, force functions need to act on position)
public void addForce(Vector3d force)
{
if (force.Length > 0.001)
{
accel = Vector3d.Add(accel, force);
}
}
public SlicedSphere(double radius, Vector3d offset, eSubdivisions subdivs, eDir[] sides, bool useDL)
: base(useDL)
{
double Diameter = radius;
PrimitiveMode = OpenTK.Graphics.OpenGL.BeginMode.Triangles;
if (sides[0] == eDir.All)
{
sides = new eDir[] { eDir.FrontTopRight,
eDir.FrontBottomRight,
eDir.FrontBottomLeft,
eDir.FrontTopLeft,
eDir.BackTopRight,
eDir.BackBottomRight,
eDir.BackBottomLeft,
eDir.BackTopLeft, };
}
VertexArray = new VertexT2dN3dV3d[sides.Length * 3];
IndexArray = new uint[sides.Length * 3];
uint counter = 0;
foreach (eDir s in sides)
{
GetDefaultVertices(s, Diameter, out VertexArray[counter + 0], out VertexArray[counter + 1], out VertexArray[counter + 2]);
IndexArray[counter + 0] = counter + 0;
IndexArray[counter + 1] = counter + 1;
IndexArray[counter + 2] = counter + 2;
counter += 3;
}
if (subdivs != eSubdivisions.Zero)
{
for (int s = 0; s < (int)subdivs; s++)
{
#region Assemble Chunks and convert to Arrays
List <Chunk> AllChunks = new List <Chunk>();
for (uint i = 0; i < IndexArray.Length; i += 3)
{
Chunk chu;
Subdivide(Diameter,
ref VertexArray[IndexArray[i + 0]],
ref VertexArray[IndexArray[i + 1]],
ref VertexArray[IndexArray[i + 2]],
out chu);
AllChunks.Add(chu);
}
Chunk.GetArray(ref AllChunks, out VertexArray, out IndexArray);
AllChunks.Clear();
#endregion Assemble Chunks and convert to Arrays
}
}
for (int i = 0; i < VertexArray.Length; i++)
{
Vector3d.Add(ref VertexArray[i].Position, ref offset, out VertexArray[i].Position);
}
}
public AABB3d At(ref Vector3d position_world)
{
var copy = this;
Vector3d.Add(ref copy.Min, ref position_world, out copy.Min);
Vector3d.Add(ref copy.Max, ref position_world, out copy.Max);
return(copy);
}
void InitLineBuffer()
{
if (lineBuffers.Count == 0)
{
int count = linePoints.Count;
PositionVertexBuffer11 lineBuffer = null;
SharpDX.Vector3[] linePointList = null;
localCenter = new Vector3d();
if (DepthBuffered)
{
// compute the local center..
foreach (Vector3d point in linePoints)
{
localCenter.Add(point);
}
localCenter.X /= count;
localCenter.Y /= count;
localCenter.Z /= count;
}
int countLeft = count;
int index = 0;
int counter = 0;
Vector3d temp;
foreach (Vector3d point in linePoints)
{
if (counter >= 100000 || linePointList == null)
{
if (lineBuffer != null)
{
lineBuffer.Unlock();
}
int thisCount = Math.Min(100000, countLeft);
countLeft -= thisCount;
lineBuffer = new PositionVertexBuffer11(thisCount, RenderContext11.PrepDevice);
linePointList = (SharpDX.Vector3[])lineBuffer.Lock(0, 0); // Lock the buffer (which will return our structs)
lineBuffers.Add(lineBuffer);
lineBufferCounts.Add(thisCount);
counter = 0;
}
temp = point - localCenter;
linePointList[counter] = temp.Vector311;
index++;
counter++;
}
lineBuffer.Unlock();
}
}
private void InitializeRoute(RenderContext renderContext)
{
triangleList = new TriangleList();
triangleList.Decay = 1000;
triangleList.Sky = this.Astronomical;
triangleList.TimeSeries = true;
triangleList.DepthBuffered = false;
triangleList.AutoTime = false;
int steps = 500;
Vector3d start = Coordinates.GeoTo3dDouble(latStart, lngStart);
Vector3d end = Coordinates.GeoTo3dDouble(latEnd, lngEnd);
Vector3d dir = Vector3d.SubtractVectors(end, start);
dir.Normalize();
Vector3d startNormal = start;
startNormal.Normalize();
Vector3d left = Vector3d.Cross(startNormal, dir);
Vector3d right = Vector3d.Cross(dir, startNormal);
left.Normalize();
right.Normalize();
left.Multiply(.001 * width);
right.Multiply(.001 * width);
Vector3d lastLeft = new Vector3d();
Vector3d lastRight = new Vector3d();
bool firstTime = true;
for (int i = 0; i <= steps; i++)
{
Vector3d v = Vector3d.Lerp(start, end, i / (float)steps);
v.Normalize();
// v.Multiply(1.1);
Vector3d cl = v;
Vector3d cr = v;
cl.Add(left);
cr.Add(right);
if (!firstTime)
{
triangleList.AddQuad(lastRight, lastLeft, cr, cl, Color, new Dates(i / (float)steps, 2));
}
else
{
firstTime = false;
}
lastLeft = cl;
lastRight = cr;
}
}
/// <summary>Expects CCW triangle order as input, emits 4 new CCW triangles.</summary>
/// <param name="first">1st output Triangle</param>
/// <param name="second">2nd output Triangle</param>
/// <param name="third">3rd output Triangle</param>
/// <param name="fourth">4th output Triangle</param>
public void SubdivideSierpinski(out TetrahedronFace first, out TetrahedronFace second, out TetrahedronFace third, out TetrahedronFace fourth)
{
Vector3d temp;
// find the 3 points AB, BC, CA
Vector3d CenterAB;
Vector3d.Add(ref this.APosition, ref this.BPosition, out temp);
Vector3d.Multiply(ref temp, 0.5f, out CenterAB);
Vector3d CenterBC;
Vector3d.Add(ref this.BPosition, ref this.CPosition, out temp);
Vector3d.Multiply(ref temp, 0.5f, out CenterBC);
Vector3d CenterCA;
Vector3d.Add(ref this.CPosition, ref this.APosition, out temp);
Vector3d.Multiply(ref temp, 0.5f, out CenterCA);
// find the 3 points AD, BD, CD
Vector3d CenterAD;
Vector3d.Lerp(ref this.APosition, ref this.DPosition, 0.5, out CenterAD);
Vector3d CenterBD;
Vector3d.Lerp(ref this.BPosition, ref this.DPosition, 0.5, out CenterBD);
Vector3d CenterCD;
Vector3d.Lerp(ref this.CPosition, ref this.DPosition, 0.5, out CenterCD);
// emit 4 new CCW triangles
first = new TetrahedronFace(ref this.APosition, ref this.ATexCoord,
ref CenterAB, ref this.BTexCoord,
ref CenterCA, ref this.CTexCoord,
ref CenterAD,
ref this.Normal);
second = new TetrahedronFace(ref CenterAB, ref this.ATexCoord,
ref this.BPosition, ref this.BTexCoord,
ref CenterBC, ref this.CTexCoord,
ref CenterBD,
ref this.Normal);
third = new TetrahedronFace(ref CenterCA, ref this.ATexCoord,
ref CenterBC, ref this.BTexCoord,
ref this.CPosition, ref this.CTexCoord,
ref CenterCD,
ref this.Normal);
fourth = new TetrahedronFace(ref CenterAD, ref this.ATexCoord,
ref CenterBD, ref this.BTexCoord,
ref CenterCD, ref this.CTexCoord,
ref this.DPosition,
ref this.Normal);
}
public bool Intersect(ref Vector3d point, ref Vector3d position)
{
Vector3d point_world;
Vector3d.Add(ref point, ref position, out point_world);
return(point_world.X >= Min.X && point_world.X <= Max.X &&
point_world.Y >= Min.Y && point_world.Y <= Max.Y &&
point_world.Z >= Min.Z && point_world.Z <= Max.Z);
}
public static Vector3d TestAddThroughInterface () {
IVector3d sum = new Vector3d();
for (int i = 0; i < BufferSize; i++) {
sum = sum.Add(Vectors[i]);
}
return (Vector3d)sum;
}
public static List <object> CSE(Vector3d r0, Vector3d v0, double dt, double mu, double x0,
double tol)
{
var rScale = r0.Magnitude();
var vScale = Math.Sqrt(mu / rScale);
var r0s = Vector3d.Divide(r0, rScale);
var v0s = Vector3d.Divide(v0, vScale);
var dts = dt * vScale / rScale;
var v2s = Math.Pow(v0.Magnitude(), 2);
var alpha = 2 - v2s;
var armd1 = v2s - 1;
var rvr0s = Vector3d.Dot(r0, v0) / Math.Sqrt(mu * rScale);
var x = x0;
if (x0 == 0)
{
x = dts * Math.Abs(alpha);
}
var ratio = 1d;
var x2 = x * x;
var z = alpha * x2;
var SCz = SnC(z);
var x2cz = x2 * SCz["C"];
var f = 0d;
var df = 0d;
while (Math.Abs(ratio) < tol)
{
f = x + rvr0s * x2cz + armd1 * x * x2 * SCz["S"] - dts;
df = x * rvr0s * (1 - z * SCz["S"]) + armd1 * x2cz + 1;
ratio = f / df;
x = x - ratio;
x2 = x * x;
z = alpha * x2;
SCz = SnC(z);
x2cz = x2 * SCz["C"];
}
var Lf = 1 - x2cz;
var Lg = dts - x2 * x * SCz["C"];
var r1 = Vector3d.Add(Vector3d.Multiply(r0s, Lf), Vector3d.Multiply(v0s, Lg));
var ir1 = 1 / r1.Magnitude();
var LfDot = ir1 * x * (z - SCz["S"] - 1);
var LgDot = 1 - x2cz * ir1;
var v1 = Vector3d.Add(Vector3d.Multiply(r0s, LfDot), Vector3d.Multiply(v0s, LgDot));
return(new List <object>
{
Vector3d.Multiply(r1, rScale),
Vector3d.Multiply(v1, vScale),
x
});
}
public static void TranslateVertices(List <Vertex> vectorlList, double x, double y, double z)
{
for (int i = 0; i < vectorlList.Count; i++)
{
Vector3d translation = new Vector3d(x, y, z);
Vertex v = vectorlList[i];
Vector3d translatedV = Vector3d.Add(v.Vector, translation);
v.Vector = translatedV;
vectorlList[i] = v;
}
}
public static Vector3d TestAddThroughInterface()
{
IVector3d sum = new Vector3d();
for (int i = 0; i < BufferSize; i++)
{
sum = sum.Add(Vectors[i]);
}
return((Vector3d)sum);
}
public void update(double damping)
{
//missing boolean - not sure if needed.
//limit acceleration to acc limit
double vectorLength = accel.Length;
double limit = vectorLength / accLimit;
if (limit != 0)
{
accel = Vector3d.Divide(accel, limit);
}
//might not work, testing
// accel /= limit;
//add accelleration to velocity
if (accel.Length > 0)
{
Vector3d newVel = Vector3d.Add(vel, accel);
vel = newVel;
}
//limit velocity to speed
if (vel.Length > 0)
{
double vectorVelLength = vel.Length;
double velLimit = vectorVelLength / spd;
Vector3d newVel2 = Vector3d.Divide(vel, velLimit);
//update velocity
vel = newVel2;
}
//update position of agent
position += vel;
age++;
//wipe accelleration vector
accel = new Vector3d();
}
/// <summary>A, B and C are the triangle whos normal is to be determined. D is the 4th Point in the Tetraeder which does not belong to the triangle.</summary>
internal static void FindNormal(ref Vector3d A, ref Vector3d B, ref Vector3d C, ref Vector3d D, out Vector3d result)
{
Vector3d temp1, temp2, temp3;
Vector3d.Subtract(ref A, ref D, out temp1);
Vector3d.Subtract(ref B, ref D, out temp2);
Vector3d.Subtract(ref C, ref D, out temp3);
Vector3d.Add(ref temp1, ref temp2, out result);
Vector3d.Add(ref result, ref temp3, out result);
result.Normalize();
}
public static Vector3d TestAdd()
{
Vector3d sum = new Vector3d();
for (int i = 0; i < BufferSize; i++)
{
sum = Vector3d.Add(sum, Vectors[i]);
}
return(sum);
}
/// <summary>Calculates the dot product of array of doubles and array of Vectors</summary>
/// <param name="u">Array of doubles (of length n)</param>
/// <param name="v">Array of Vectors (of same length, n)</param>
/// <returns>Dot product</returns>
/// <remarks>
/// Does not check that the arrays have the same length
/// </remarks>
public static Vector3d Multiply(double[] u, Vector3d[] v)
{
int num = u.Length;
Vector3d vector = default(Vector3d);
for (int i = 0; i < num; i++)
{
vector = vector.Add(v[i].Multiply(u[i]));
}
return(vector);
}
private static Point3f calculateCentre(List <Vertex> tFaceVerts)
{
Vector3d centreVector = new Vector3d(0, 0, 0);
for (int i = 0; i < tFaceVerts.Count; i++)
{
Vector3d toAdd = new Vector3d(tFaceVerts[i].position);
centreVector = Vector3d.Add(centreVector, toAdd);
}
centreVector = Vector3d.Divide(centreVector, tFaceVerts.Count);
return(new Point3f((float)centreVector.X, (float)centreVector.Y, (float)centreVector.Z));
}
public AABB updateBounds()
{
//is called once the extents are set (ie, called in the constructor also)
if (extent != null)
{
//construct a new vector based on the smallest components of both vectors
this.min = Vector3d.Subtract(centrePoint, extent);
this.max = Vector3d.Add(centrePoint, extent);
}
return(this);
}
public override Vector3d AvoidEdges(IAgent agent, double distance)
{
Vector3d desired = new Vector3d();
Vector3d avoidVec, parVec;
Curve[] overlapCrvs;
Point3d[] intersectPts;
Curve[] feelers = GetFeelerCrvs(agent, distance, false);
int count = 0;
foreach (Brep[] borderWalls in borderWallsArray)
{
foreach (Brep brep in borderWalls)
{
foreach (Curve feeler in feelers)
{
//Check feeler intersection with each brep face
foreach (BrepFace face in brep.Faces)
{
Intersection.CurveBrepFace(feeler, face, Constants.AbsoluteTolerance, out overlapCrvs, out intersectPts);
if (intersectPts.Length > 0)
{
Point3d testPt = feeler.PointAtEnd;
double u, v;
face.ClosestPoint(testPt, out u, out v);
Vector3d normal = face.NormalAt(u, v);
normal.Reverse();
Vector.GetProjectionComponents(normal, agent.Velocity, out parVec, out avoidVec);
avoidVec.Unitize();
//weight by distance
if (!agent.Position3D.DistanceTo(intersectPts[0]).Equals(0))
{
avoidVec = Vector3d.Divide(avoidVec, agent.Position3D.DistanceTo(intersectPts[0]));
}
desired = Vector3d.Add(desired, avoidVec);
count++;
break; //Break when we hit a face
}
}
}
}
}
if (count > 0)
{
desired = Vector3d.Divide(desired, count);
}
return(desired);
}
public SkyLabel(RenderContext11 renderContext, double ra, double dec, string text, LabelSytle style, double distance)
{
RA = ra;
Dec = dec;
Text = text;
Style = style;
Distance = distance;
if (texture == null)
{
texture = Texture11.FromBitmap(Properties.Resources.circle, 0);
}
Vector3d up = new Vector3d();
Vector3d textPos = new Vector3d();
if (Earth3d.MainWindow.SolarSystemMode)
{
pos = Coordinates.RADecTo3d(ra, -dec, distance);
up = Coordinates.RADecTo3d(ra, -dec + 90, distance);
pos.RotateX(Coordinates.MeanObliquityOfEcliptic(SpaceTimeController.JNow) / 180.0 * Math.PI);
pos.Add(Planets.GetPlanet3dLocation(SolarSystemObjects.Earth));
up.RotateX(Coordinates.MeanObliquityOfEcliptic(SpaceTimeController.JNow) / 180.0 * Math.PI);
up.Add(Planets.GetPlanet3dLocation(SolarSystemObjects.Earth));
}
else
{
pos = Coordinates.RADecTo3d(ra + 12, dec, distance);
textPos = Coordinates.RADecTo3d(ra + 12, dec + 2, distance);
up = Coordinates.RADecTo3d(ra + 12, dec + 92, distance);
}
center = new Vector3(9, 9, 0);
textBatch = new Text3dBatch(24);
if (style == LabelSytle.Telrad)
{
// Telrad-style labels are always screen-aligned
Text3d t3 = new Text3d(new Vector3d(0, 0, 0.1), new Vector3d(0, 1, 0), text, 20, .01);
t3.alignment = Text3d.Alignment.Left;
textBatch.Add(t3);
}
else
{
up.Normalize();
textPos.Normalize();
textBatch.Add(new Text3d(textPos, up, text, 20, .0005));
}
}
private void Slerp(ref Vector3d a, ref Vector3d b, double factor, out Vector3d result)
{
double t1;
Vector3d.Dot(ref a, ref b, out t1);
double theta = System.Math.Acos(t1);
double temp = 1.0 / System.Math.Sin(theta);
double t2 = System.Math.Sin((1.0 - factor) * theta) * temp;
double t3 = System.Math.Sin(factor * theta) * temp;
Vector3d v1 = Vector3d.Multiply(a, t2);
Vector3d v2 = Vector3d.Multiply(b, t3);
result = Vector3d.Add(v1, v2);
}
//private bool isAutoReducing = false;
//constructor // :base is the same as super to pass down to AABB constructor
public AgentOctree(AgentOctree p, Vector3d o, double halfSize) :
base(Vector3d.Add(o, new Vector3d(halfSize, halfSize, halfSize)), new Vector3d(halfSize, halfSize, halfSize))
{
this.parent = p;
this.halfSize = halfSize;
this.size = halfSize + halfSize;
this.offset = o;
this.numChildren = 0;
if (parent != null)
{
depth = parent.depth + 1;
minNodeSize = parent.minNodeSize;
}
}
internal float calculateThicknessOffset(float thickness, float minDistance)
{
//get the normals and the middle normal - should walk for both directions, should do, they are just vectors.
Vector3d n1 = leftFace.faceNormal;
Vector3d n2 = rightFace.faceNormal;
Vector3d nMid = Vector3d.Add(n1, n2);
nMid.Unitize();
double alpha = Vector3d.VectorAngle(n1, nMid);
double lFull = ((minDistance / 2) / Math.Sin(alpha)) + (thickness / 2);
double offsetDistance = (lFull * Math.Sin(alpha)) / (Math.Sin(Math.PI / 2 - alpha));
return((float)offsetDistance);
}
public List <Point3d> halfEdgesPosTag(PlanktonMesh pMesh)
{
List <Point3d> halfedgesPos = new List <Point3d>();
foreach (PlanktonHalfedge pHe in pMesh.Halfedges)
{
int startVertex = pHe.StartVertex;
int endVertex = pMesh.Halfedges[pHe.NextHalfedge].StartVertex;
Vector3d vecStart = new Vector3d(pMesh.Vertices[startVertex].X, pMesh.Vertices[startVertex].Y, pMesh.Vertices[startVertex].Z);
Vector3d vecEnd = new Vector3d(pMesh.Vertices[endVertex].X, pMesh.Vertices[endVertex].Y, pMesh.Vertices[endVertex].Z);
Vector3d vecEdge = Vector3d.Subtract(vecEnd, vecStart);
Vector3d vecEdgeOneThird = Vector3d.Multiply(0.33, vecEdge);
Vector3d vecPos = Vector3d.Add(vecStart, vecEdgeOneThird);
halfedgesPos.Add(new Point3d(vecPos.X, vecPos.Y, vecPos.Z));
}
return(halfedgesPos);
}
virtual public void Run()
{
Velocity = Vector3d.Add(Velocity, Acceleration);
Velocity3D = MapTo3D(Velocity);
Acceleration3D = MapTo3D(Acceleration);
Point3d position = Position;
position.Transform(Transform.Translation(Velocity));
bool wrapped = false;
if (Environment.Wrap)
{
Position = Environment.WrapPoint(position, out wrapped);
}
else
{
Position = Environment.ClosestPointOnRef(position);
}
Point3d position3D = Position3D;
position3D.Transform(Transform.Translation(Velocity3D));
if (wrapped)
{
Velocity3D = Environment.Orient(Velocity, Position3D);
//Velocity3D = Velocity;
}
Position3D = position3D;
Position3DHistory.Add(Position3D, wrapped);
PreviousAcceleration3D = Acceleration3D;
PreviousAcceleration = Acceleration;
UpdateOrientation();
Acceleration = Vector3d.Zero;
Lifespan--;
}
public bool addPoint(Agent p)
{
//check if pt is inside cube - write code for contains point
if (containsPoint(p))
{
if (halfSize <= minNodeSize)
{
if (points == null)
{
points = new List <Agent>();
}
points.Add(p);
return(true);
}
else
{
Vector3d plocal = Vector3d.Subtract(p.position, offset);
if (children == null)
{
children = new AgentOctree[8];
}
int octant = getOctantID(plocal);
if (children[octant] == null)
{
Vector3d off = Vector3d.Add(offset, new Vector3d(
(octant & 1) != 0 ? halfSize : 0,
(octant & 2) != 0 ? halfSize : 0,
(octant & 4) != 0 ? halfSize : 0));
children[octant] = new AgentOctree(this, off, halfSize * 0.5);
numChildren++;
}
return(children[octant].addPoint(p));
}
}
return(false);
}