3 files changed, 361 insertions, 10 deletions

diff --git a/OpenSim/Region/Physics/Meshing/Extruder.cs b/OpenSim/Region/Physics/Meshing/Extruder.cs
index 4d2ed4b..c7dcf33 100644
--- a/OpenSim/Region/Physics/Meshing/Extruder.cs
+++ b/OpenSim/Region/Physics/Meshing/Extruder.cs
@@ -39,9 +39,15 @@ namespace OpenSim.Region.Physics.Meshing
         public float taperTopFactorY = 1f;
         public float taperBotFactorX = 1f;
         public float taperBotFactorY = 1f;
+
         public float pushX = 0f;
         public float pushY = 0f;
 
+        // twist amount in radians.  NOT DEGREES.
+        public float twistTop = 0;
+        public float twistBot = 0;
+        public float twistMid = 0;
+
         public Mesh Extrude(Mesh m)
         {
             startParameter = float.MinValue;
@@ -50,8 +56,12 @@ namespace OpenSim.Region.Physics.Meshing
             Mesh result = new Mesh();
 
             Mesh workingPlus = m.Clone();
+            Mesh workingMiddle = m.Clone();
             Mesh workingMinus = m.Clone();
 
+            Quaternion tt = new Quaternion();
+            Vertex v2 = new Vertex(0, 0, 0);
+
             foreach (Vertex v in workingPlus.vertices)
             {
                 if (v == null)
@@ -68,8 +78,44 @@ namespace OpenSim.Region.Physics.Meshing
                 //Push the top of the object over by the Top Shear amount
                 v.X += pushX * size.X;
                 v.Y += pushY * size.X;
+
+                if (twistTop != 0)
+                {
+                    // twist and shout
+                    tt = new Quaternion(new Vertex(0, 0, 1), twistTop);
+                    v2 = v * tt;
+                    v.X = v2.X;
+                    v.Y = v2.Y;
+                    v.Z = v2.Z;
+                }
             }
 
+            foreach (Vertex v in workingMiddle.vertices)
+            {
+                if (v == null)
+                    continue;
+
+                // This is the top
+                // Set the Z + .5 to match the rest of the scale of the mesh
+                // Scale it by Size, and Taper the scaling
+                v.Z *= size.Z;
+                v.X *= (size.X * ((taperTopFactorX + taperBotFactorX) /2));
+                v.Y *= (size.Y * ((taperTopFactorY + taperBotFactorY) / 2));
+
+                v.X += (pushX / 2) * size.X;
+                v.Y += (pushY / 2) * size.X;
+                //Push the top of the object over by the Top Shear amount
+                if (twistMid != 0)
+                {
+                    // twist and shout
+                    tt = new Quaternion(new Vertex(0, 0, 1), twistMid);
+                    v2 = v * tt;
+                    v.X = v2.X;
+                    v.Y = v2.Y;
+                    v.Z = v2.Z;
+                }
+              
+            }
             foreach (Vertex v in workingMinus.vertices)
             {
                 if (v == null)
@@ -80,6 +126,16 @@ namespace OpenSim.Region.Physics.Meshing
                 v.X *= (size.X * taperBotFactorX);
                 v.Y *= (size.Y * taperBotFactorY);
                 v.Z *= size.Z;
+
+                if (twistBot != 0)
+                {
+                    // twist and shout
+                    tt = new Quaternion(new Vertex(0, 0, 1), twistBot);
+                    v2 = v * tt;
+                    v.X = v2.X;
+                    v.Y = v2.Y;
+                    v.Z = v2.Z;
+                }
             }
 
             foreach (Triangle t in workingMinus.triangles)
@@ -88,9 +144,47 @@ namespace OpenSim.Region.Physics.Meshing
             }
 
             result.Append(workingMinus);
-            result.Append(workingPlus);
+
+            result.Append(workingMiddle);
+
 
             int iLastNull = 0;
+
+            for (int i = 0; i < workingMiddle.vertices.Count; i++)
+            {
+                int iNext = (i + 1);
+
+                if (workingMiddle.vertices[i] == null) // Can't make a simplex here
+                {
+                    iLastNull = i + 1;
+                    continue;
+                }
+
+                if (i == workingMiddle.vertices.Count - 1) // End of list
+                {
+                    iNext = iLastNull;
+                }
+
+                if (workingMiddle.vertices[iNext] == null) // Null means wrap to begin of last segment
+                {
+                    iNext = iLastNull;
+                }
+
+                Triangle tSide;
+                tSide = new Triangle(workingMiddle.vertices[i], workingMinus.vertices[i], workingMiddle.vertices[iNext]);
+                result.Add(tSide);
+
+                tSide =
+                    new Triangle(workingMiddle.vertices[iNext], workingMinus.vertices[i], workingMinus.vertices[iNext]);
+                result.Add(tSide);
+            }
+            //foreach (Triangle t in workingPlus.triangles)
+            //{
+                //t.invertNormal();
+           // }
+            result.Append(workingPlus);
+
+            iLastNull = 0;
             for (int i = 0; i < workingPlus.vertices.Count; i++)
             {
                 int iNext = (i + 1);
@@ -112,14 +206,28 @@ namespace OpenSim.Region.Physics.Meshing
                 }
 
                 Triangle tSide;
-                tSide = new Triangle(workingPlus.vertices[i], workingMinus.vertices[i], workingPlus.vertices[iNext]);
+                tSide = new Triangle(workingPlus.vertices[i], workingMiddle.vertices[i], workingPlus.vertices[iNext]);
                 result.Add(tSide);
 
                 tSide =
-                    new Triangle(workingPlus.vertices[iNext], workingMinus.vertices[i], workingMinus.vertices[iNext]);
+                    new Triangle(workingPlus.vertices[iNext], workingMiddle.vertices[i], workingMiddle.vertices[iNext]);
                 result.Add(tSide);
             }
-
+            if (twistMid != 0)
+            {
+                foreach (Vertex v in result.vertices)
+                {
+                    // twist and shout
+                    if (v != null)
+                    {
+                        tt = new Quaternion(new Vertex(0, 0, -1), twistMid*2);
+                        v2 = v * tt;
+                        v.X = v2.X;
+                        v.Y = v2.Y;
+                        v.Z = v2.Z;
+                    }
+                }
+            }
             return result;
         }
     }
diff --git a/OpenSim/Region/Physics/Meshing/HelperTypes.cs b/OpenSim/Region/Physics/Meshing/HelperTypes.cs
index 7b17a3f..efc5968 100644
--- a/OpenSim/Region/Physics/Meshing/HelperTypes.cs
+++ b/OpenSim/Region/Physics/Meshing/HelperTypes.cs
@@ -32,16 +32,136 @@ using System.Globalization;
 using OpenSim.Region.Physics.Manager;
 using OpenSim.Region.Physics.Meshing;
 
-public class Vertex : PhysicsVector, IComparable<Vertex>
+public class Quaternion
 {
-    public Vertex(float x, float y, float z)
-        : base(x, y, z)
+    public float x = 0;
+    public float y = 0;
+    public float z = 0;
+    public float w = 1;
+
+    public Quaternion()
     {
-    }
 
+    }
+    public Quaternion(float x1, float y1, float z1, float w1)
+    {
+        x = x1; y = y1; z = z1; w = w1;
+    }
+    public Quaternion(Vertex axis, float angle)
+    {
+        // using (* 0.5) instead of (/2) 
+        w = (float)Math.Cos(angle * 0.5f);
+        x = axis.X * (float)Math.Sin(angle * 0.5f);
+        y = axis.Y * (float)Math.Sin(angle * 0.5f);
+        z = axis.Z * (float)Math.Sin(angle * 0.5f);
+        normalize();
+    }
+    public static Quaternion operator *(Quaternion a, Quaternion b)
+    {
+        Quaternion c = new Quaternion();
+        c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
+        c.y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z;
+        c.z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x;
+        c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
+        return c;
+    }
+    
+    
+    public Matrix4 computeMatrix()
+    {
+        return new Matrix4(this);
+    }
+    public void normalize()
+    {
+        float mag = length();
+       
+        w /= mag;
+        x /= mag;
+        y /= mag;
+        z /= mag;
+    }
     public float length()
     {
-        return (float)Math.Sqrt(X * X + Y * Y + Z * Z);
+        return (float)Math.Sqrt(w * w + x * x + y * y + z * z);
+    }
+}
+public class Matrix4
+{
+    public float m00 = 0;
+    public float m01 = 0;
+    public float m02 = 0;
+    public float m03 = 0;
+    public float m10 = 0;
+    public float m11 = 0;
+    public float m12 = 0;
+    public float m13 = 0;
+    public float m20 = 0;
+    public float m21 = 0;
+    public float m22 = 0;
+    public float m23 = 0;
+    public float m30 = 0;
+    public float m31 = 0;
+    public float m32 = 0;
+    public float m33 = 1;
+
+    public Matrix4(float m001, float m011, float m021, float m031, float m101, float m111, float m121, float m131, float m201, float m211, float m221, float m231, float m301, float m311, float m321, float m331)
+    {
+        m00 = m001;
+        m01 = m011;
+        m02 = m021;
+        m03 = m031;
+        m10 = m101;
+        m11 = m111;
+        m12 = m121;
+        m13 = m131;
+        m20 = m201;
+        m21 = m211;
+        m22 = m221;
+        m23 = m231;
+        m30 = m301;
+        m31 = m311;
+        m32 = m321;
+        m33 = m331;
+    }
+    public Matrix4()
+    {
+    }
+    public Matrix4(Quaternion r)
+    {
+        m00 = 1 - (2 * (r.y * r.y)) - (2 * (r.z * r.z));
+        m01 = (r.x * r.y * 2) - (r.w * r.z * 2);
+        m02 = (r.x * r.z * 2) + (r.w * r.y * 2);
+        m03 = 0f;
+        m10 = (r.x * r.y * 2) + (r.w * r.z * 2);
+        m11 = 1 - (2 * (r.x * r.x)) - (2 * (r.z * r.z));
+        m12 = (r.y * r.z * 2) - (r.w * r.x * 2);
+        m13 = 0f;
+        m20 = (r.x * r.z * 2) - (r.w * r.y * 2);
+        m21 = (r.y * r.z * 2) - (r.w * r.x * 2);
+        m22 = 1 - (2 * (r.x * r.x)) - (2 * (r.y * r.y));
+        m23 = 0f;
+        m30 = 0f;
+        m31 = 0f;
+        m32 = 0f;
+        m33 = 1f;
+    }
+    public Vertex transform(Vertex o)
+    {
+        Vertex r = new Vertex(0,0,0);
+        // w value implicitly 1 therefore the last + m3x actually represents (m3x * o.W) = m3x
+        // in calculating the dot product.
+        r.X = (m00 * o.X) + (m10 * o.Y) + (m20 * o.Z) + m30;
+        r.Y = (m01 * o.X) + (m11 * o.Y) + (m21 * o.Z) + m31;
+        r.Z = (m02 * o.X) + (m12 * o.Y) + (m22 * o.Z) + m32;
+        return r;
+    }
+}
+
+public class Vertex : PhysicsVector, IComparable<Vertex>
+{
+    public Vertex(float x, float y, float z)
+        : base(x, y, z)
+    {
     }
 
     public Vertex normalize()
@@ -62,6 +182,12 @@ public class Vertex : PhysicsVector, IComparable<Vertex>
         return new Vertex(Y * v.Z - Z * v.Y, Z * v.X - X * v.Z, X * v.Y - Y * v.X);
     }
 
+    public static Vertex operator *(Vertex v, Quaternion q)
+    {
+        Matrix4 tm = q.computeMatrix();
+        return tm.transform(v);
+    }
+
     public static Vertex operator +(Vertex v1, Vertex v2)
     {
         return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index 80f5bd1..d4b6a50 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -58,6 +58,7 @@ namespace OpenSim.Region.Physics.Meshing
         // raw files can be imported by blender so a visual inspection of the results can be done
         //        const string baseDir = "rawFiles";
         private const string baseDir = null; //"rawFiles";
+        private const float DEG_TO_RAD = 0.01745329238f;
 
 // TODO: unused
 //         private static void IntersectionParameterPD(PhysicsVector p1, PhysicsVector r1, PhysicsVector p2,
@@ -195,6 +196,7 @@ namespace OpenSim.Region.Physics.Meshing
 
                     break;
 
+                case ProfileShape.HalfCircle:
                 case ProfileShape.Circle:
                     if (pbs.PathCurve == (byte)Extrusion.Straight)
                     {
@@ -359,6 +361,8 @@ namespace OpenSim.Region.Physics.Meshing
             UInt16 taperY = primShape.PathScaleY;
             UInt16 pathShearX = primShape.PathShearX;
             UInt16 pathShearY = primShape.PathShearY;
+            Int16 twistTop = primShape.PathTwistBegin;
+            Int16 twistBot = primShape.PathTwist;
             
 
             //m_log.Error("pathShear:" + primShape.PathShearX.ToString() + "," + primShape.PathShearY.ToString());
@@ -531,7 +535,42 @@ namespace OpenSim.Region.Physics.Meshing
                     //m_log.Warn("pushY: " + extr.pushY);
                 }
             }
-            
+
+            if (twistTop != 0)
+            {
+                extr.twistTop = 180 * ((float)twistTop / 100);
+                if (extr.twistTop > 0)
+                {
+                    extr.twistTop = 360 - (-1 * extr.twistTop);
+
+                }
+                
+
+                extr.twistTop = (float)(extr.twistTop * DEG_TO_RAD);
+            }
+
+            float twistMid = ((twistTop + twistBot) * 0.5f);
+
+            if (twistMid != 0)
+            {
+                extr.twistMid = 180 * ((float)twistMid / 100);
+                if (extr.twistMid > 0)
+                {
+                    extr.twistMid = 360 - (-1 * extr.twistMid);
+                }
+                extr.twistMid = (float)(extr.twistMid * DEG_TO_RAD);
+            }
+
+            if (twistBot != 0)
+            {
+                extr.twistBot = 180 * ((float)twistBot / 100);
+                if (extr.twistBot > 0)
+                {
+                    extr.twistBot = 360 - (-1 * extr.twistBot);
+                }
+                extr.twistBot = (float)(extr.twistBot * DEG_TO_RAD);
+            }
+
             Mesh result = extr.Extrude(m);
             result.DumpRaw(baseDir, primName, "Z extruded");
             return result;
@@ -540,6 +579,7 @@ namespace OpenSim.Region.Physics.Meshing
         private static Mesh CreateCyllinderMesh(String primName, PrimitiveBaseShape primShape, PhysicsVector size)
         // Builds the z (+ and -) surfaces of a box shaped prim
         {
+
             UInt16 hollowFactor = primShape.ProfileHollow;
             UInt16 profileBegin = primShape.ProfileBegin;
             UInt16 profileEnd = primShape.ProfileEnd;
@@ -547,6 +587,9 @@ namespace OpenSim.Region.Physics.Meshing
             UInt16 taperY = primShape.PathScaleY;
             UInt16 pathShearX = primShape.PathShearX;
             UInt16 pathShearY = primShape.PathShearY;
+            Int16 twistBot = primShape.PathTwist;
+            Int16 twistTop = primShape.PathTwistBegin;
+            
 
             // Procedure: This is based on the fact that the upper (plus) and lower (minus) Z-surface
             // of a block are basically the same
@@ -797,8 +840,45 @@ namespace OpenSim.Region.Physics.Meshing
                     extr.pushY = (float)pathShearY / 100;
                     //m_log.Warn("pushY: " + extr.pushY);
                 }
+                
+            }
+
+            if (twistTop != 0)
+            {
+                extr.twistTop = 180 * ((float)twistTop / 100);
+                if (extr.twistTop > 0)
+                {
+                    extr.twistTop = 360 - (-1 * extr.twistTop);
+                    
+                }
+                
+
+                extr.twistTop = (float)(extr.twistTop * DEG_TO_RAD);
+            }
+
+            float twistMid = ((twistTop + twistBot) * 0.5f);
+
+            if (twistMid != 0)
+            {
+                extr.twistMid = 180 * ((float)twistMid / 100);
+                if (extr.twistMid > 0)
+                {
+                    extr.twistMid = 360 - (-1 * extr.twistMid);
+                }
+                extr.twistMid = (float)(extr.twistMid * DEG_TO_RAD);
+            }
+            
+            if (twistBot != 0)
+            {
+                extr.twistBot = 180 * ((float)twistBot / 100);
+                if (extr.twistBot > 0)
+                {
+                    extr.twistBot = 360 - (-1 * extr.twistBot);
+                }
+                extr.twistBot = (float)(extr.twistBot * DEG_TO_RAD);
             }
 
+            //System.Console.WriteLine("[MESH]: twistTop = " + twistTop.ToString() + "|" + extr.twistTop.ToString() + ", twistMid = " + twistMid.ToString() + "|" + extr.twistMid.ToString() + ", twistbot = " + twistBot.ToString() + "|" + extr.twistBot.ToString());
             Mesh result = extr.Extrude(m);
             result.DumpRaw(baseDir, primName, "Z extruded");
             return result;
@@ -815,6 +895,8 @@ namespace OpenSim.Region.Physics.Meshing
             UInt16 pathShearX = primShape.PathShearX;
             UInt16 pathShearY = primShape.PathShearY;
 
+            Int16 twistTop = primShape.PathTwistBegin;
+            Int16 twistBot = primShape.PathTwist;
             //m_log.Error("pathShear:" + primShape.PathShearX.ToString() + "," + primShape.PathShearY.ToString());
             //m_log.Error("pathTaper:" + primShape.PathTaperX.ToString() + "," + primShape.PathTaperY.ToString());
             //m_log.Error("ProfileBegin:" + primShape.ProfileBegin.ToString() + "," + primShape.ProfileBegin.ToString());
@@ -984,6 +1066,41 @@ namespace OpenSim.Region.Physics.Meshing
                 }
             }
 
+            if (twistTop != 0)
+            {
+                extr.twistTop = 180 * ((float)twistTop / 100);
+                if (extr.twistTop > 0)
+                {
+                    extr.twistTop = 360 - (-1 * extr.twistTop);
+
+                }
+                
+
+                extr.twistTop = (float)(extr.twistTop * DEG_TO_RAD);
+            }
+
+            float twistMid = ((twistTop + twistBot) * 0.5f);
+
+            if (twistMid != 0)
+            {
+                extr.twistMid = 180 * ((float)twistMid / 100);
+                if (extr.twistMid > 0)
+                {
+                    extr.twistMid = 360 - (-1 * extr.twistMid);
+                }
+                extr.twistMid = (float)(extr.twistMid * DEG_TO_RAD);
+            }
+
+            if (twistBot != 0)
+            {
+                extr.twistBot = 180 * ((float)twistBot / 100);
+                if (extr.twistBot > 0)
+                {
+                    extr.twistBot = 360 - (-1 * extr.twistBot);
+                }
+                extr.twistBot = (float)(extr.twistBot * DEG_TO_RAD);
+            }
+
             Mesh result = extr.Extrude(m);
             result.DumpRaw(baseDir, primName, "Z extruded");
             return result;