public static void SLerp(out idVec3 t, idVec3 v1, idVec3 v2, float l)
{
if (l <= 0.0f)
{
t = v1; return;
}
else if (l >= 1.0f)
{
t = v2; return;
}
float cosom = v1 * v2;
float scale0;
float scale1;
if ((1.0f - cosom) > LERP_DELTA)
{
float omega = (float)Math.Acos(cosom);
float sinom = (float)Math.Sin(omega);
scale0 = (float)Math.Sin((1.0f - l) * omega) / sinom;
scale1 = (float)Math.Sin(l * omega) / sinom;
}
else
{
scale0 = 1.0f - l;
scale1 = l;
}
t = (v1 * scale0 + v2 * scale1);
}
public idVec3 Cross(idVec3 a, idVec3 b)
{
x = a.y * b.z - a.z * b.y;
y = a.z * b.x - a.x * b.z;
z = a.x * b.y - a.y * b.x;
return(this);
}
public void Clamp(idVec3 min, idVec3 max)
{
if (x < min.x)
{
x = min.x;
}
else if (x > max.x)
{
x = max.x;
}
if (y < min.y)
{
y = min.y;
}
else if (y > max.y)
{
y = max.y;
}
if (z < min.z)
{
z = min.z;
}
else if (z > max.y)
{
z = max.z;
}
}
public void Set(idVec3 rotationOrigin, idVec3 rotationVec, float rotationAngle)
{
origin = rotationOrigin;
vec = rotationVec;
angle = rotationAngle;
axisValid = false;
}
public idVec3 Cross(idVec3 a, idVec3 b)
{
x = a.y * b.z - a.z * b.y;
y = a.z * b.x - a.x * b.z;
z = a.x * b.y - a.y * b.x;
return this;
}
public void Set(idVec3 rotationOrigin, idVec3 rotationVec, float rotationAngle)
{
origin = rotationOrigin;
vec = rotationVec;
angle = rotationAngle;
axisValid = false;
}
public void OrthogonalBasis(idVec3 left, idVec3 up)
{
if (idMath.Fabs(z) > 0.7f)
{
float l = y * y + z * z;
float s = idMath.InvSqrt(l);
up.x = 0;
up.y = z * s;
up.z = -y * s;
left.x = l * s;
left.y = -x * up[2];
left.z = x * up[1];
}
else
{
float l = x * x + y * y;
float s = idMath.InvSqrt(l);
left.x = -y * s;
left.y = x * s;
left.z = 0;
up.x = -z * left[1];
up.y = z * left[0];
up.z = l * s;
}
}
public void RotatePoint(ref idVec3 point)
{
if (!axisValid)
{
ToMat3();
}
point = ((point - origin) * axis + origin);
}
public idVec3 ToAngularVelocity()
{
idVec3 vec = new idVec3();
vec.x = x;
vec.y = y;
vec.z = z;
vec.Normalize();
return(vec * idMath.ACos(w));
}
// extra
public idVec3 opMul(ref idVec3 vec, ref idMat3 mat)
{
idVec3[] mat_mat = mat.mat;
float x = mat_mat[0].x * vec.x + mat_mat[1].x * vec.y + mat_mat[2].x * vec.z;
float y = mat_mat[0].y * vec.x + mat_mat[1].y * vec.y + mat_mat[2].y * vec.z;
vec.z = mat_mat[0].z * vec.x + mat_mat[1].z * vec.y + mat_mat[2].z * vec.z;
vec.x = x;
vec.y = y;
return(vec);
}
public idRotation ToRotation()
{
if (pitch == 0.0f)
{
if (yaw == 0.0f)
{
return(new idRotation(idvec3.origin, new idVec3(-1.0f, 0.0f, 0.0f), roll));
}
if (roll == 0.0f)
{
return(new idRotation(idVec3.origin, new idVec3(0.0f, 0.0f, -1.0f), yaw));
}
}
else if (yaw == 0.0f && roll == 0.0f)
{
return(new idRotation(idVec3.origin, new idVec3(0.0f, -1.0f, 0.0f), pitch));
}
float sx, cx, sy, cy, sz, cz;
idMath.SinCos(DEG2RAD(yaw) * 0.5f, out sz, out cz);
idMath.SinCos(DEG2RAD(pitch) * 0.5f, out sy, out cy);
idMath.SinCos(DEG2RAD(roll) * 0.5f, out sx, out cx);
float sxcy = sx * cy;
float cxcy = cx * cy;
float sxsy = sx * sy;
float cxsy = cx * sy;
//
idVec3 vec = new idVec3();
vec.x = cxsy * sz - sxcy * cz;
vec.y = -cxsy * cz - sxcy * sz;
vec.z = sxsy * cz - cxcy * sz;
float w = cxcy * cz + sxsy * sz;
float angle = idMath.ACos(w);
if (angle == 0.0f)
{
vec.Set(0.0f, 0.0f, 1.0f);
}
else
{
//vec *= (1.0f / sin( angle ));
vec.Normalize();
vec.FixDegenerateNormal();
angle *= 2.0f * idMath.M_RAD2DEG;
}
return(new idRotation(idVec3.origin, vec, angle));
}
public static bool ProjectAlongPlane(idVec3 t, idVec3 normal, float epsilon, float overBounce)
{
idVec3 cross = t.Cross(normal).Cross(t);
// normalize so a fixed epsilon can be used
cross.Normalize();
float len = normal * cross;
if (idMath.Fabs(len) < epsilon)
{
return(false);
}
cross.opMul(overBounce * (normal * t) / len);
t.opSub(cross);
return(true);
}
public static void ProjectOntoPlane(ref idVec3 t, idVec3 normal, float overBounce)
{
float backoff = t * normal;
if (overBounce != 1.0)
{
if (backoff < 0)
{
backoff *= overBounce;
}
else
{
backoff /= overBounce;
}
}
t.opSub(normal * backoff);
}
public void NormalVectors(ref idVec3 left, ref idVec3 down)
{
float d = x * x + y * y;
if (d == 0)
{
left.x = 1;
left.y = 0;
left.z = 0;
}
else
{
d = idMath.InvSqrt(d);
left.x = -y * d;
left.y = x * d;
left.z = 0;
}
down = left.Cross(this);
}
public void ToVectors(ref idVec3 forward, ref idVec3 right, ref idVec3 up)
{
float sr, sp, sy, cr, cp, cy;
idMath.SinCos(DEG2RAD(yaw), out sy, out cy);
idMath.SinCos(DEG2RAD(pitch), out sp, out cp);
idMath.SinCos(DEG2RAD(roll), out sr, out cr);
if (forward != null)
{
forward.Set(cp * cy, cp * sy, -sp);
}
if (right != null)
{
right.Set(-sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp);
}
if (up != null)
{
up.Set(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp);
}
}
public idMat4(ref idMat3 rotation, ref idVec3 translation)
{
idVec3[] rotation_mat = rotation.mat;
// NOTE: idMat3 is transposed because it is column-major
mat[0].x = rotation_mat[0].x;
mat[0].y = rotation_mat[1].x;
mat[0].z = rotation_mat[2].x;
mat[0].w = translation.x;
mat[1].x = rotation_mat[0].y;
mat[1].y = rotation_mat[1].y;
mat[1].z = rotation_mat[2].y;
mat[1].w = translation.y;
mat[2].x = rotation_mat[0].z;
mat[2].y = rotation_mat[1].z;
mat[2].z = rotation_mat[2].z;
mat[2].w = translation.z;
mat[3].x = 0.0f;
mat[3].y = 0.0f;
mat[3].z = 0.0f;
mat[3].w = 1.0f;
}
public idMat4(ref idMat3 rotation, ref idVec3 translation)
{
idVec3[] rotation_mat = rotation.mat;
// NOTE: idMat3 is transposed because it is column-major
mat[0].x = rotation_mat[0].x;
mat[0].y = rotation_mat[1].x;
mat[0].z = rotation_mat[2].x;
mat[0].w = translation.x;
mat[1].x = rotation_mat[0].y;
mat[1].y = rotation_mat[1].y;
mat[1].z = rotation_mat[2].y;
mat[1].w = translation.y;
mat[2].x = rotation_mat[0].z;
mat[2].y = rotation_mat[1].z;
mat[2].z = rotation_mat[2].z;
mat[2].w = translation.z;
mat[3].x = 0.0f;
mat[3].y = 0.0f;
mat[3].z = 0.0f;
mat[3].w = 1.0f;
}
public idRotation ToRotation()
{
idVec3 vec = new idVec3();
vec.x = x;
vec.y = y;
vec.z = z;
float angle = idMath.ACos(w);
if (angle == 0.0f)
{
vec.Set(0.0f, 0.0f, 1.0f);
}
else
{
//vec *= (1.0f / sin( angle ));
vec.Normalize();
vec.FixDegenerateNormal();
angle *= 2.0f * idMath.M_RAD2DEG;
}
return(new idRotation(idVec3.origin, vec, angle));
}
public idVec3 opMul(idVec3 v, idRotation r)
{
v = r * v; return(v);
}
public void ToVectors(ref idVec3 forward, ref idVec3 right, ref idVec3 up)
{
float sr, sp, sy, cr, cp, cy;
idMath.SinCos(DEG2RAD(yaw), out sy, out cy);
idMath.SinCos(DEG2RAD(pitch), out sp, out cp);
idMath.SinCos(DEG2RAD(roll), out sr, out cr);
if (forward != null) forward.Set(cp * cy, cp * sy, -sp);
if (right != null) right.Set(-sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp);
if (up != null) up.Set(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp);
}
public idAngles(ref idVec3 v)
{
this.pitch = v.x;
this.yaw = v.y;
this.roll = v.z;
}
public bool Compare(ref idVec3 a, float epsilon) { return (idMath.Fabs(x - a.x) <= epsilon && idMath.Fabs(y - a.y) <= epsilon && idMath.Fabs(z - a.z) <= epsilon); }
static void UnpackColor(uint color, ref idVec3 unpackedColor)
{
unpackedColor.Set(((color >> 0) & 255) * (1.0f / 255.0f),
((color >> 8) & 255) * (1.0f / 255.0f),
((color >> 16) & 255) * (1.0f / 255.0f));
}
// extra
public idVec3 opMul(ref idVec3 vec, ref idMat3 mat)
{
idVec3[] mat_mat = mat.mat;
float x = mat_mat[0].x * vec.x + mat_mat[1].x * vec.y + mat_mat[2].x * vec.z;
float y = mat_mat[0].y * vec.x + mat_mat[1].y * vec.y + mat_mat[2].y * vec.z;
vec.z = mat_mat[0].z * vec.x + mat_mat[1].z * vec.y + mat_mat[2].z * vec.z;
vec.x = x;
vec.y = y;
return vec;
}
public idVec3 Cross(idVec3 a)
{
return(new idVec3(y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x));
}
public void RotatePoint(ref idVec3 point)
{
if (!axisValid)
ToMat3();
point = ((point - origin) * axis + origin);
}
public idVec3 opDiv(idVec3 a)
{
x /= a.x; y /= a.y; z /= a.z; return(this);
}
public idVec5(idVec3 xyz, idVec2 st)
{
x = xyz.x; y = xyz.y; z = xyz.z; s = st.x; t = st.y;
}
public idVec3 opMul(idVec3 v, idRotation r) { v = r * v; return v; }
public bool Compare(ref idVec3 a)
{
return(x == a.x && y == a.y && z == a.z);
}
public bool Compare(ref idVec3 a, float epsilon)
{
return(idMath.Fabs(x - a.x) <= epsilon && idMath.Fabs(y - a.y) <= epsilon && idMath.Fabs(z - a.z) <= epsilon);
}
public idAngles(ref idVec3 v)
{
this.pitch = v.x;
this.yaw = v.y;
this.roll = v.z;
}
public void ProjectVector(ref idVec3 src, ref idVec3 dst)
{
dst.x = src * mat[0];
dst.y = src * mat[1];
dst.z = src * mat[2];
}
public void SetOrigin(idVec3 rotationOrigin)
{
origin = rotationOrigin;
}
public idRotation ToRotation()
{
idVec3 vec = new idVec3();
vec.x = x;
vec.y = y;
vec.z = z;
float angle = idMath.ACos(w);
if (angle == 0.0f)
{
vec.Set(0.0f, 0.0f, 1.0f);
}
else
{
//vec *= (1.0f / sin( angle ));
vec.Normalize();
vec.FixDegenerateNormal();
angle *= 2.0f * idMath.M_RAD2DEG;
}
return new idRotation(idVec3.origin, vec, angle);
}
public void SetVec(idVec3 rotationVec)
{
vec = rotationVec;
axisValid = false;
}
public void UnprojectVector(ref idVec3 src, ref idVec3 dst)
{
dst = mat[0] * src.x + mat[1] * src.y + mat[2] * src.z;
}
public idVec3 opSub(idVec3 a)
{
x -= a.x; y -= a.y; z -= a.z; return(this);
}
public bool Compare(ref idVec3 a) { return (x == a.x && y == a.y && z == a.z); }
// packs color floats in the range [0,1] into an integer
static uint PackColor(ref idVec3 color)
{
uint dx = ColorFloatToByte(color.x);
uint dy = ColorFloatToByte(color.y);
uint dz = ColorFloatToByte(color.z);
uint dw = ColorFloatToByte(color.w);
return (dx << 0) | (dy << 8) | (dz << 16) | (dw << 24);
}
public idVec3 Cross(idVec3 a) { return new idVec3(y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x); }
public static void SLerp(out idVec3 t, idVec3 v1, idVec3 v2, float l)
{
if (l <= 0.0f) { t = v1; return; }
else if (l >= 1.0f) { t = v2; return; }
float cosom = v1 * v2;
float scale0;
float scale1;
if ((1.0f - cosom) > LERP_DELTA)
{
float omega = (float)Math.Acos(cosom);
float sinom = (float)Math.Sin(omega);
scale0 = (float)Math.Sin((1.0f - l) * omega) / sinom;
scale1 = (float)Math.Sin(l * omega) / sinom;
}
else
{
scale0 = 1.0f - l;
scale1 = l;
}
t = (v1 * scale0 + v2 * scale1);
}
public void Clamp(idVec3 min, idVec3 max)
{
if (x < min.x) x = min.x;
else if (x > max.x) x = max.x;
if (y < min.y) y = min.y;
else if (y > max.y) y = max.y;
if (z < min.z) z = min.z;
else if (z > max.y) z = max.z;
}
public void NormalVectors(ref idVec3 left, ref idVec3 down)
{
float d = x * x + y * y;
if (d == 0)
{
left.x = 1;
left.y = 0;
left.z = 0;
}
else
{
d = idMath.InvSqrt(d);
left.x = -y * d;
left.y = x * d;
left.z = 0;
}
down = left.Cross(this);
}
public idRotation ToRotation()
{
if (pitch == 0.0f)
{
if (yaw == 0.0f) return new idRotation(idvec3.origin, new idVec3(-1.0f, 0.0f, 0.0f), roll);
if (roll == 0.0f) return new idRotation(idVec3.origin, new idVec3(0.0f, 0.0f, -1.0f), yaw);
}
else if (yaw == 0.0f && roll == 0.0f) return new idRotation(idVec3.origin, new idVec3(0.0f, -1.0f, 0.0f), pitch);
float sx, cx, sy, cy, sz, cz;
idMath.SinCos(DEG2RAD(yaw) * 0.5f, out sz, out cz);
idMath.SinCos(DEG2RAD(pitch) * 0.5f, out sy, out cy);
idMath.SinCos(DEG2RAD(roll) * 0.5f, out sx, out cx);
float sxcy = sx * cy;
float cxcy = cx * cy;
float sxsy = sx * sy;
float cxsy = cx * sy;
//
idVec3 vec = new idVec3();
vec.x = cxsy * sz - sxcy * cz;
vec.y = -cxsy * cz - sxcy * sz;
vec.z = sxsy * cz - cxcy * sz;
float w = cxcy * cz + sxsy * sz;
float angle = idMath.ACos(w);
if (angle == 0.0f)
vec.Set(0.0f, 0.0f, 1.0f);
else
{
//vec *= (1.0f / sin( angle ));
vec.Normalize();
vec.FixDegenerateNormal();
angle *= 2.0f * idMath.M_RAD2DEG;
}
return new idRotation(idVec3.origin, vec, angle);
}
public void OrthogonalBasis(idVec3 left, idVec3 up)
{
if (idMath.Fabs(z) > 0.7f)
{
float l = y * y + z * z;
float s = idMath.InvSqrt(l);
up.x = 0;
up.y = z * s;
up.z = -y * s;
left.x = l * s;
left.y = -x * up[2];
left.z = x * up[1];
}
else
{
float l = x * x + y * y;
float s = idMath.InvSqrt(l);
left.x = -y * s;
left.y = x * s;
left.z = 0;
up.x = -z * left[1];
up.y = z * left[0];
up.z = l * s;
}
}
public void SetVec(idVec3 rotationVec)
{
vec = rotationVec;
axisValid = false;
}
public static void ProjectOntoPlane(ref idVec3 t, idVec3 normal, float overBounce)
{
float backoff = t * normal;
if (overBounce != 1.0)
{
if (backoff < 0)
backoff *= overBounce;
else
backoff /= overBounce;
}
t.opSub(normal * backoff);
}
public void UnprojectVector(ref idVec3 src, ref idVec3 dst)
{
dst = mat[0] * src.x + mat[1] * src.y + mat[2] * src.z;
}
public static bool ProjectAlongPlane(idVec3 t, idVec3 normal, float epsilon, float overBounce)
{
idVec3 cross = t.Cross(normal).Cross(t);
// normalize so a fixed epsilon can be used
cross.Normalize();
float len = normal * cross;
if (idMath.Fabs(len) < epsilon)
return false;
cross.opMul(overBounce * (normal * t) / len);
t.opSub(cross);
return true;
}
public void SetOrigin(idVec3 rotationOrigin)
{
origin = rotationOrigin;
}
public idVec3 opAdd(idVec3 a) { x += a.x; y += a.y; z += a.z; return this; }
public idVec3 ToAngularVelocity()
{
idVec3 vec = new idVec3();
vec.x = x;
vec.y = y;
vec.z = z;
vec.Normalize();
return vec * idMath.ACos(w);
}
public idVec3 opSub(idVec3 a) { x -= a.x; y -= a.y; z -= a.z; return this; }
public void ProjectVector(ref idVec3 src, ref idVec3 dst)
{
dst.x = src * mat[0];
dst.y = src * mat[1];
dst.z = src * mat[2];
}
public idVec3 opDiv(idVec3 a) { x /= a.x; y /= a.y; z /= a.z; return this; }
public idVec5(idVec3 xyz, idVec2 st) { x = xyz.x; y = xyz.y; z = xyz.z; s = st.x; t = st.y; }
public idVec3 opAdd(idVec3 a)
{
x += a.x; y += a.y; z += a.z; return(this);
}
