public LSL_Vector llVecNorm(LSL_Vector v) { if (!ScriptProtection.CheckThreatLevel(ThreatLevel.None, "LSL", m_host, "LSL", m_itemID)) { return(new LSL_Vector()); } return(LSL_Vector.Norm(v)); }
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); }