// if this position is outside sphere, push it back in by one diameter public Vec3 SphericalWraparound(Vec3 center, float radius) { Vec3 offset = this - center; float r = offset.Length(); if (r > radius) return this + ((offset / r) * radius * -2); else return this; }
public Vec3(Vec3 other) { X = other.X; Y = other.Y; Z = other.Z; }
// utility member functions used in OpenSteer // return component of vector parallel to a unit basis vector // IMPORTANT NOTE: assumes "basis" has unit magnitude (length == 1) public Vec3 ParallelComponent(Vec3 unitBasis) { float projection = Dot(unitBasis); return unitBasis * projection; }
// return component of vector perpendicular to a unit basis vector // IMPORTANT NOTE: assumes "basis" has unit magnitude(length==1) public Vec3 PerpendicularComponent(Vec3 unitBasis) { return (this) - ParallelComponent(unitBasis); }
// cross product (modify "this" to be A x B) // [XXX side effecting -- deprecate this function? XXX] //FIXME: make this a static returning the Cross product public void Cross(Vec3 vector1, Vec3 vector2) { this = new Vec3((vector1.Y * vector2.Z) - (vector1.Z * vector2.Y), (vector1.Z * vector2.X) - (vector1.X * vector2.Z), (vector1.X * vector2.Y) - (vector1.Y * vector2.X)); }
// dot product public float Dot(Vec3 vector2) { return (X * vector2.X) + (Y * vector2.Y) + (Z * vector2.Z); }
// ---------------------------------------------------------------------------- // Enforce a lower bound on the angle by which a given arbitrary vector // diviates from a given reference direction (specified by a unit basis // vector). The effect is to clip the "source" vector to be outside a cone // defined by the basis and an angle. public static Vec3 LimitMinDeviationAngle(Vec3 source, float cosineOfConeAngle, Vec3 basis) { return LimitDeviationAngleUtility(false, // force source OUTSIDE cone source, cosineOfConeAngle, basis); }
// ---------------------------------------------------------------------------- // Returns a position randomly distributed on a disk of unit radius // on the XZ (Y=0) plane, centered at the origin. Orientation will be // random and length will range between 0 and 1 public static Vec3 RandomVectorOnUnitRadiusXZDisk() { Vec3 v = new Vec3(); do { v.Set((Utilities.Random() * 2) - 1, 0, (Utilities.Random() * 2) - 1); } while (v.Length() >= 1); return v; }
// ---------------------------------------------------------------------------- // Enforce an upper bound on the angle by which a given arbitrary vector // diviates from a given reference direction (specified by a unit basis // vector). The effect is to clip the "source" vector to be inside a cone // defined by the basis and an angle. public static Vec3 LimitMaxDeviationAngle(Vec3 source, float cosineOfConeAngle, Vec3 basis) { return LimitDeviationAngleUtility(true, // force source INSIDE cone source, cosineOfConeAngle, basis); }
// ---------------------------------------------------------------------------- // used by limitMaxDeviationAngle / limitMinDeviationAngle below public static Vec3 LimitDeviationAngleUtility(bool insideOrOutside, Vec3 source, float cosineOfConeAngle, Vec3 basis) { // immediately return zero length input vectors float sourceLength = source.Length(); if (sourceLength == 0) return source; // measure the angular diviation of "source" from "basis" Vec3 direction = source / sourceLength; float cosineOfSourceAngle = direction.Dot(basis); // Simply return "source" if it already meets the angle criteria. // (note: we hope this top "if" gets compiled out since the flag // is a constant when the function is inlined into its caller) if (insideOrOutside) { // source vector is already inside the cone, just return it if (cosineOfSourceAngle >= cosineOfConeAngle) return source; } else { // source vector is already outside the cone, just return it if (cosineOfSourceAngle <= cosineOfConeAngle) return source; } // find the portion of "source" that is perpendicular to "basis" Vec3 perp = source.PerpendicularComponent(basis); // normalize that perpendicular Vec3 unitPerp = perp.Normalize(); // construct a new vector whose length equals the source vector, // and lies on the intersection of a plane (formed the source and // basis vectors) and a cone (whose axis is "basis" and whose // angle corresponds to cosineOfConeAngle) float perpDist = (float)Math.Sqrt(1 - (cosineOfConeAngle * cosineOfConeAngle)); Vec3 c0 = basis * cosineOfConeAngle; Vec3 c1 = unitPerp * perpDist; return (c0 + c1) * sourceLength; }
// ---------------------------------------------------------------------------- // given a vector, return a vector perpendicular to it (note that this // arbitrarily selects one of the infinitude of perpendicular vectors) public static Vec3 FindPerpendicularIn3d(Vec3 direction) { // to be filled in: Vec3 quasiPerp; // a direction which is "almost perpendicular" Vec3 result = new Vec3(); // the computed perpendicular to be returned // three mutually perpendicular basis vectors Vec3 i = new Vec3(1, 0, 0); Vec3 j = new Vec3(0, 1, 0); Vec3 k = new Vec3(0, 0, 1); // measure the projection of "direction" onto each of the axes float id = i.Dot(direction); float jd = j.Dot(direction); float kd = k.Dot(direction); // set quasiPerp to the basis which is least parallel to "direction" if ((id <= jd) && (id <= kd)) { quasiPerp = i; // projection onto i was the smallest } else { if ((jd <= id) && (jd <= kd)) quasiPerp = j; // projection onto j was the smallest else quasiPerp = k; // projection onto k was the smallest } // return the cross product (direction x quasiPerp) // which is guaranteed to be perpendicular to both of them result.Cross(direction, quasiPerp); return result; }
// ---------------------------------------------------------------------------- // Returns the distance between a point and a line. The line is defined in // terms of a point on the line ("lineOrigin") and a UNIT vector parallel to // the line ("lineUnitTangent") public static float DistanceFromLine(Vec3 point, Vec3 lineOrigin, Vec3 lineUnitTangent) { Vec3 offset = point - lineOrigin; Vec3 perp = offset.PerpendicularComponent(lineUnitTangent); return perp.Length(); }
public static float Distance(Vec3 vector1, Vec3 vector2) { return (vector1 - vector2).Length(); }