// Xantor 29/apr/2008
// converts a Quaternion to X,Y,Z axis rotations
public LSL_Vector llRot2Axis(LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return new LSL_Vector();
double x, y, z;
if (rot.s > 1) // normalization needed
{
double length = Math.Sqrt(rot.x*rot.x + rot.y*rot.y +
rot.z*rot.z + rot.s*rot.s);
if (length == 0)
return new LSL_Vector(0, 0, 0);
length = 1/length;
rot.x *= length;
rot.y *= length;
rot.z *= length;
rot.s *= length;
}
// double angle = 2 * Math.Acos(rot.s);
double s = Math.Sqrt(1 - rot.s*rot.s);
if (s < 0.001)
{
x = 1;
y = z = 0;
}
else
{
s = 1/s;
x = rot.x*s; // normalise axis
y = rot.y*s;
z = rot.z*s;
}
return new LSL_Vector(x, y, z);
}
//Now we start getting into quaternions which means sin/cos, matrices and vectors. ckrinke
// Old implementation of llRot2Euler. Normalization not required as Atan2 function will
// only return values >= -PI (-180 degrees) and <= PI (180 degrees).
public LSL_Vector llRot2Euler(LSL_Rotation r)
{
//This implementation is from http://lslwiki.net/lslwiki/wakka.php?wakka=LibraryRotationFunctions. ckrinke
LSL_Rotation t = new LSL_Rotation(r.x*r.x, r.y*r.y, r.z*r.z, r.s*r.s);
double m = (t.x + t.y + t.z + t.s);
if (m == 0) return new LSL_Vector();
double n = 2*(r.y*r.s + r.x*r.z);
double p = m*m - n*n;
if (p > 0)
return new LSL_Vector(Math.Atan2(2.0*(r.x*r.s - r.y*r.z), (-t.x - t.y + t.z + t.s)),
Math.Atan2(n, Math.Sqrt(p)),
Math.Atan2(2.0*(r.z*r.s - r.x*r.y), (t.x - t.y - t.z + t.s)));
if (n > 0)
return new LSL_Vector(0.0, Math.PI*0.5, Math.Atan2((r.z*r.s + r.x*r.y), 0.5 - t.x - t.z));
return new LSL_Vector(0.0, -Math.PI*0.5, Math.Atan2((r.z*r.s + r.x*r.y), 0.5 - t.x - t.z));
}
public DateTime osRezObject(string inventory, LSL_Vector pos, LSL_Vector vel, LSL_Rotation rot, int param,
LSL_Integer isRezAtRoot, LSL_Integer doRecoil, LSL_Integer SetDieAtEdge,
LSL_Integer CheckPos)
{
InitLSL();
return m_LSL_Api.llRezPrim(inventory, pos, vel, rot, param, isRezAtRoot == 1, doRecoil == 1,
SetDieAtEdge == 1, CheckPos == 1);
}
// Returns the angle of a quaternion (see llRot2Axis for the axis)
public LSL_Float llRot2Angle(LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return new LSL_Float();
if (rot.s > 1) // normalization needed
{
double length = Math.Sqrt(rot.x*rot.x + rot.y*rot.y +
rot.z*rot.z + rot.s*rot.s);
if (length == 0)
return 0;
// rot.x /= length;
// rot.y /= length;
// rot.z /= length;
rot.s /= length;
}
double angle = 2*Math.Acos(rot.s);
return angle;
}
public DateTime llSetLocalRot(LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return DateTime.Now;
SetRot(m_host, Rot2Quaternion(rot));
return PScriptSleep(200);
}
public void llSetVehicleRotationParam(int param, LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) return;
if (m_host.ParentEntity != null)
{
if (!m_host.ParentEntity.IsDeleted)
{
m_host.ParentEntity.RootChild.SetVehicleRotationParam(param,
Rot2Quaternion(rot));
}
}
}
// convert a LSL_Rotation to a Quaternion
protected Quaternion Rot2Quaternion(LSL_Rotation r)
{
Quaternion q = new Quaternion((float) r.x, (float) r.y, (float) r.z, (float) r.s);
q.Normalize();
return q;
}
public void llRotLookAt(LSL_Rotation target, double strength, double damping)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) return;
Quaternion rot = new Quaternion((float) target.x, (float) target.y, (float) target.z, (float) target.s);
m_host.RotLookAt(rot, (float) strength, (float) damping);
}
public void botSetRot(LSL_Key npc, LSL_Rotation rotation)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.Moderate, "botStandUp", m_host, "bot", m_itemID)) return;
IScenePresence sp = World.GetScenePresence(UUID.Parse(npc));
if (sp == null)
return;
UUID npcId;
if (!UUID.TryParse(npc.m_string, out npcId))
return;
if (sp != null)
sp.Rotation = rotation.ToQuaternion();
}
public LSL_Float llAngleBetween(LSL_Rotation a, LSL_Rotation b)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return new LSL_Float();
double aa = (a.x*a.x + a.y*a.y + a.z*a.z + a.s*a.s);
double bb = (b.x*b.x + b.y*b.y + b.z*b.z + b.s*b.s);
double aa_bb = aa*bb;
if (aa_bb == 0) return 0.0;
double ab = (a.x*b.x + a.y*b.y + a.z*b.z + a.s*b.s);
double quotient = (ab*ab)/aa_bb;
if (quotient >= 1.0) return 0.0;
return Math.Acos(2*quotient - 1);
}
public void osForceDropAttachmentAt(LSL_Vector pos, LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.High, "osForceDropAttachmentAt", m_host, "OSSL", m_itemID)) return;
DropAttachmentAt(false, pos, rot);
}
public void osDropAttachmentAt(LSL_Vector pos, LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.Moderate, "osDropAttachmentAt", m_host, "OSSL", m_itemID)) return;
DropAttachmentAt(true, pos, rot);
}
protected void DropAttachmentAt(bool checkPerms, LSL_Vector pos, LSL_Rotation rot)
{
if (checkPerms && ShoutErrorOnLackingOwnerPerms(ScriptBaseClass.PERMISSION_ATTACH, "Cannot drop attachment"))
{
return;
}
IAttachmentsModule attachmentsModule = World.RequestModuleInterface<IAttachmentsModule>();
IScenePresence sp = attachmentsModule == null ? null : World.GetScenePresence(m_host.OwnerID);
if (attachmentsModule != null && sp != null)
{
attachmentsModule.DetachSingleAttachmentToGround(m_host.ParentEntity.UUID, sp.ControllingClient, pos.ToVector3(), rot.ToQuaternion());
}
}
public LSL_Vector llRot2Up(LSL_Rotation r)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return new LSL_Vector();
double m = r.x*r.x + r.y*r.y + r.z*r.z + r.s*r.s;
// m is always greater than zero
// if m is not equal to 1 then Rotation needs to be normalized
if (Math.Abs(1.0 - m) > 0.000001) // allow a little slop here for calculation precision
{
m = 1.0/Math.Sqrt(m);
r.x *= m;
r.y *= m;
r.z *= m;
r.s *= m;
}
// Fast Algebric Calculations instead of Vectors & Quaternions Product
double x = 2*(r.x*r.z + r.y*r.s);
double y = 2*(-r.x*r.s + r.y*r.z);
double z = -r.x*r.x - r.y*r.y + r.z*r.z + r.s*r.s;
return (new LSL_Vector(x, y, z));
}
private void SetLinkRot(ISceneChildEntity obj, LSL_Rotation rot)
{
if (obj.ParentID == 0)
{
// special case: If we are root, rotate complete SOG to new rotation
SetRot(obj, Rot2Quaternion(rot));
}
else
{
// we are a child. The rotation values will be set to the one of root modified by rot, as in SL. Don't ask.
ISceneEntity group = obj.ParentEntity;
if (group != null) // a bit paranoid, maybe
{
ISceneChildEntity rootPart = group.RootChild;
if (rootPart != null) // again, better safe than sorry
{
SetRot(obj, rootPart.GetRotationOffset()*Rot2Quaternion(rot));
}
}
}
}
public LSL_Rotation llRotBetween(LSL_Vector a, LSL_Vector b)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return new LSL_Rotation();
//A and B should both be normalized
LSL_Rotation rotBetween;
// Check for zero vectors. If either is zero, return zero rotation. Otherwise,
// continue calculation.
if (a == new LSL_Vector(0.0f, 0.0f, 0.0f) || b == new LSL_Vector(0.0f, 0.0f, 0.0f))
{
rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
}
else
{
a = LSL_Vector.Norm(a);
b = LSL_Vector.Norm(b);
double dotProduct = LSL_Vector.Dot(a, b);
// There are two degenerate cases possible. These are for vectors 180 or
// 0 degrees apart. These have to be detected and handled individually.
//
// Check for vectors 180 degrees apart.
// A dot product of -1 would mean the angle between vectors is 180 degrees.
if (dotProduct < -0.9999999f)
{
// First assume X axis is orthogonal to the vectors.
LSL_Vector orthoVector = new LSL_Vector(1.0f, 0.0f, 0.0f);
orthoVector = orthoVector - a*(a.x/LSL_Vector.Dot(a, a));
// Check for near zero vector. A very small non-zero number here will create
// a rotation in an undesired direction.
rotBetween = LSL_Vector.Mag(orthoVector) > 0.0001
? new LSL_Rotation(orthoVector.x, orthoVector.y, orthoVector.z, 0.0f)
: new LSL_Rotation(0.0f, 0.0f, 1.0f, 0.0f);
}
// Check for parallel vectors.
// A dot product of 1 would mean the angle between vectors is 0 degrees.
else if (dotProduct > 0.9999999f)
{
// Set zero rotation.
rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
}
else
{
// All special checks have been performed so get the axis of rotation.
LSL_Vector crossProduct = LSL_Vector.Cross(a, b);
// Quarternion s value is the length of the unit vector + dot product.
double qs = 1.0 + dotProduct;
rotBetween = new LSL_Rotation(crossProduct.x, crossProduct.y, crossProduct.z, qs);
// Normalize the rotation.
double mag = LSL_Rotation.Mag(rotBetween);
// We shouldn't have to worry about a divide by zero here. The qs value will be
// non-zero because we already know if we're here, then the dotProduct is not -1 so
// qs will not be zero. Also, we've already handled the input vectors being zero so the
// crossProduct vector should also not be zero.
rotBetween.x = rotBetween.x/mag;
rotBetween.y = rotBetween.y/mag;
rotBetween.z = rotBetween.z/mag;
rotBetween.s = rotBetween.s/mag;
// Check for undefined values and set zero rotation if any found. This code might not actually be required
// any longer since zero vectors are checked for at the top.
if (Double.IsNaN(rotBetween.x) || Double.IsNaN(rotBetween.y) || Double.IsNaN(rotBetween.z) ||
Double.IsNaN(rotBetween.s))
{
rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
}
}
}
return rotBetween;
}
public void llLinkRotLookAt(LSL_Integer link, LSL_Rotation target, double strength, double damping)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) return;
Quaternion rot = new Quaternion((float) target.x, (float) target.y, (float) target.z, (float) target.s);
List<ISceneChildEntity> parts = GetLinkParts(link);
foreach (ISceneChildEntity part in parts)
part.RotLookAt(rot, (float) strength, (float) damping);
}
public LSL_Integer llRotTarget(LSL_Rotation rot, double error)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) return 0;
return
m_host.registerRotTargetWaypoint(
new Quaternion((float) rot.x, (float) rot.y, (float) rot.z, (float) rot.s), (float) error);
}
public void llLinkSitTarget(LSL_Integer link, LSL_Vector offset, LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) return;
// LSL quaternions can normalize to 0, normal Quaternions can't.
if (rot.s == 0 && rot.x == 0 && rot.y == 0 && rot.z == 0)
rot.z = 1; // ZERO_ROTATION = 0,0,0,1
List<ISceneChildEntity> entities = GetLinkParts(link);
if (entities.Count == 0)
return;
entities[0].SitTargetPosition = new Vector3((float) offset.x, (float) offset.y, (float) offset.z);
entities[0].SitTargetOrientation = Rot2Quaternion(rot);
}
public DateTime llSetRot(LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID))
return DateTime.Now;
// try to let this work as in SL...
SetLinkRot(m_host, rot);
return PScriptSleep(200);
}
public DateTime llRezObject(string inventory, LSL_Vector pos, LSL_Vector vel, LSL_Rotation rot, int param)
{
return llRezPrim(inventory, pos, vel, rot, param, false, true, true, true);
}
public void llSitTarget(LSL_Vector offset, LSL_Rotation rot)
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) return;
// LSL quaternions can normalize to 0, normal Quaternions can't.
if (rot.s == 0 && rot.x == 0 && rot.y == 0 && rot.z == 0)
rot.z = 1; // ZERO_ROTATION = 0,0,0,1
m_host.SitTargetPosition = new Vector3((float) offset.x, (float) offset.y, (float) offset.z);
m_host.SitTargetOrientation = Rot2Quaternion(rot);
}
public LSL_Rotation aaGetTextColor()
{
if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "aaGetTextColor", m_host, "AA", m_itemID))
return new LSL_Rotation();
LSL_Rotation v = new LSL_Rotation(m_host.Color.R, m_host.Color.G, m_host.Color.B, m_host.Color.A);
return v;
}
