// 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();
}
