public LSL_Vector llGroundNormal(LSL_Vector offset)
{
m_host.AddScriptLPS(1);
Vector3 pos = m_host.GetWorldPosition() + (Vector3)offset;
// Clamp to valid position
if (pos.X < 0)
pos.X = 0;
else if (pos.X >= World.Heightmap.Width)
pos.X = World.Heightmap.Width - 1;
if (pos.Y < 0)
pos.Y = 0;
else if (pos.Y >= World.Heightmap.Height)
pos.Y = World.Heightmap.Height - 1;
//Find two points in addition to the position to define a plane
Vector3 p0 = new Vector3(pos.X, pos.Y,
(float)World.Heightmap[(int)pos.X, (int)pos.Y]);
Vector3 p1 = new Vector3();
Vector3 p2 = new Vector3();
if ((pos.X + 1.0f) >= World.Heightmap.Width)
p1 = new Vector3(pos.X + 1.0f, pos.Y,
(float)World.Heightmap[(int)pos.X, (int)pos.Y]);
else
p1 = new Vector3(pos.X + 1.0f, pos.Y,
(float)World.Heightmap[(int)(pos.X + 1.0f), (int)pos.Y]);
if ((pos.Y + 1.0f) >= World.Heightmap.Height)
p2 = new Vector3(pos.X, pos.Y + 1.0f,
(float)World.Heightmap[(int)pos.X, (int)pos.Y]);
else
p2 = new Vector3(pos.X, pos.Y + 1.0f,
(float)World.Heightmap[(int)pos.X, (int)(pos.Y + 1.0f)]);
//Find normalized vectors from p0 to p1 and p0 to p2
Vector3 v0 = new Vector3(p1.X - p0.X, p1.Y - p0.Y, p1.Z - p0.Z);
Vector3 v1 = new Vector3(p2.X - p0.X, p2.Y - p0.Y, p2.Z - p0.Z);
v0.Normalize();
v1.Normalize();
//Find the cross product of the vectors (the slope normal).
Vector3 vsn = new Vector3();
vsn.X = (v0.Y * v1.Z) - (v0.Z * v1.Y);
vsn.Y = (v0.Z * v1.X) - (v0.X * v1.Z);
vsn.Z = (v0.X * v1.Y) - (v0.Y * v1.X);
vsn.Normalize();
//I believe the crossproduct of two normalized vectors is a normalized vector so
//this normalization may be overkill
return new LSL_Vector(vsn);
}