Ticket #1988: anchor.diff

binaries/data/mods/public/simulation/templates/template_unit_champion_cavalry.xml

@@ -42 +42 @@
   <Obstruction>
     <Unit radius="1.0"/>
   </Obstruction>
+  <Position>
+    <Anchor>pitch</Anchor>
+  </Position>
   <Selectable>
     <Overlay>
       <Texture>

binaries/data/mods/public/simulation/templates/template_unit_champion_elephant.xml

@@ -31 +31 @@
   <Obstruction>
     <Unit radius="4.0"/>
   </Obstruction>
+  <Position>
+    <Anchor>pitch</Anchor>
+  </Position>
   <Selectable>
     <Overlay>
       <Texture>

binaries/data/mods/public/simulation/templates/units/maur_support_elephant.xml

@@ -38 +38 @@
     <stone>10</stone>
     <metal>10</metal>
   </Loot>
+  <Position>
+    <Anchor>pitch</Anchor>
+  </Position>
   <ResourceDropsite>
     <Types>food wood stone metal</Types>
   </ResourceDropsite>

source/simulation2/components/CCmpPosition.cpp

@@ -30 +30 @@
 #include "maths/MathUtil.h"
 #include "maths/Matrix3D.h"
 #include "maths/Vector3D.h"
+#include "maths/Vector2D.h"
 #include "ps/CLogger.h"
 
 /**
@@ -58 +59 @@
         UPRIGHT = 0,
         PITCH = 1,
         PITCH_ROLL = 2,
+        ROLL=3,
     } m_AnchorType;
 
     bool m_Floating;
@@ -72 +74 @@
     entity_pos_t m_YOffset;
     bool m_RelativeToGround; // whether m_YOffset is relative to terrain/water plane, or an absolute height
 
-    entity_angle_t m_RotX, m_RotY, m_RotZ;
+    CTerrain* m_Terrain;
+
+    entity_angle_t m_RotX, m_RotY, m_RotZ, m_LastRotX, m_LastRotZ;
     float m_InterpolatedRotY, m_PrevInterpolatedRotY; // not serialized
+    bool recalculateXZRotation;
 
+    int widthInTiles, depthInTiles;
+
     static std::string GetSchema()
     {
         return
@@ -89 +96 @@
                 "<choice>"
                     "<value a:help='Always stand straight up'>upright</value>"
                     "<value a:help='Rotate backwards and forwards to follow the terrain'>pitch</value>"
+                    "<value a:help='Rotate sideways follow the terrain'>roll</value>"
                     "<value a:help='Rotate in all direction to follow the terrain'>pitch-roll</value>"
                 "</choice>"
             "</element>"
@@ -110 +118 @@
             m_AnchorType = PITCH;
         else if (anchor == L"pitch-roll")
             m_AnchorType = PITCH_ROLL;
+        else if (anchor == L"roll")
+            m_AnchorType = ROLL;
         else
             m_AnchorType = UPRIGHT;
 
@@ -123 +133 @@
 
         m_RotX = m_RotY = m_RotZ = entity_angle_t::FromInt(0);
         m_InterpolatedRotY = m_PrevInterpolatedRotY = 0;
+
+        m_LastRotX = entity_angle_t::Zero();
+        m_LastRotZ = entity_angle_t::Zero();
+
+        m_Terrain = &GetSimContext().GetTerrain();
+
+        recalculateXZRotation = true;
     }
 
     virtual void Deinit()
@@ -197 +214 @@
 
     virtual void MoveTo(entity_pos_t x, entity_pos_t z)
     {
+        
         m_X = x;
         m_Z = z;
 
@@ -207 +225 @@
             m_LastZ = m_PrevZ = m_Z;
         }
 
+        recalculateXZRotation = true;
+
         AdvertisePositionChanges();
     }
 
@@ -216 +236 @@
         m_LastZ = m_PrevZ = m_Z = z;
         m_InWorld = true;
 
+        recalculateXZRotation = true;
+
         AdvertisePositionChanges();
     }
 
@@ -319 +341 @@
 
     virtual void TurnTo(entity_angle_t y)
     {
+
         m_RotY = y;
 
         AdvertisePositionChanges();
@@ -330 +353 @@
         m_InterpolatedRotY = m_RotY.ToFloat();
         m_PrevInterpolatedRotY = m_InterpolatedRotY;
 
+        recalculateXZRotation = true;
+
         AdvertisePositionChanges();
     }
 
@@ -371 +396 @@
         rotY = m_InterpolatedRotY;
     }
 
+
+    virtual void UpdateXZRotation()
+    {
+
+        if (m_AnchorType == UPRIGHT)
+            // change nothing if anchor is upright 
+            return;
+        
+        // TODO average normal (average all the tiles?) for big units or for buildings
+        CFixedVector3D normal;
+
+        m_Terrain->CalcNormalFixed((m_X / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), (m_Z / (int)TERRAIN_TILE_SIZE).ToInt_RoundToZero(), normal);
+        
+        // rotate the normal so the positive x direction is in the direction of the unit
+        CFixedVector2D projected = CFixedVector2D(normal.X, normal.Z);
+        projected = projected.Rotate(-entity_pos_t::FromFloat(m_InterpolatedRotY));
+
+        normal.X = projected.X;
+        normal.Z = projected.Y;
+
+        if (m_AnchorType == PITCH || m_AnchorType == PITCH_ROLL)
+            // project and calculate the angle
+            m_RotX = -atan2_approx(normal.Z, normal.Y);
+
+        if (m_AnchorType == ROLL || m_AnchorType == PITCH_ROLL)
+            // project and calculate the angle 
+            m_RotZ = atan2_approx(normal.X,normal.Y);
+        
+        return;
+    }
+
     virtual CMatrix3D GetInterpolatedTransform(float frameOffset, bool forceFloating)
     {
+
+
         if (!m_InWorld)
         {
             LOGERROR(L"CCmpPosition::GetInterpolatedTransform called on entity when IsInWorld is false");
@@ -401 +459 @@
 
         float y = baseY + m_YOffset.ToFloat();
 
-        // TODO: do something with m_AnchorType
 
-        CMatrix3D m;
-        m.SetXRotation(m_RotX.ToFloat());
-        m.RotateZ(m_RotZ.ToFloat());
+        CMatrix3D m;
+        
+        // linear interpolation is good enough (for RotX/Z). 
+        // As you always stay close to zero angle.  
+        m.SetXRotation(Interpolate(m_LastRotX.ToFloat(), m_RotX.ToFloat(), frameOffset));
+        m.RotateZ(Interpolate(m_LastRotZ.ToFloat(), m_RotZ.ToFloat(), frameOffset));
+    
         m.RotateY(rotY + (float)M_PI);
         m.Translate(CVector3D(x, y, z));
         
@@ -418 +479 @@
         {
         case MT_Interpolate:
         {
+
             const CMessageInterpolate& msgData = static_cast<const CMessageInterpolate&> (msg);
 
             float rotY = m_RotY.ToFloat();
-            float delta = rotY - m_InterpolatedRotY;
-            // Wrap delta to -M_PI..M_PI
-            delta = fmodf(delta + (float)M_PI, 2*(float)M_PI); // range -2PI..2PI
-            if (delta < 0) delta += 2*(float)M_PI; // range 0..2PI
-            delta -= (float)M_PI; // range -M_PI..M_PI
-            // Clamp to max rate
-            float deltaClamped = clamp(delta, -m_RotYSpeed*msgData.deltaSimTime, +m_RotYSpeed*msgData.deltaSimTime);
-            // Calculate new orientation, in a peculiar way in order to make sure the
-            // result gets close to m_orientation (rather than being n*2*M_PI out)
-            m_InterpolatedRotY = rotY + deltaClamped - delta;
+            if (rotY != m_InterpolatedRotY)
+            {
+                float delta = rotY - m_InterpolatedRotY;
+                // Wrap delta to -M_PI..M_PI
+                delta = fmodf(delta + (float)M_PI, 2*(float)M_PI); // range -2PI..2PI
+                if (delta < 0) delta += 2*(float)M_PI; // range 0..2PI
+                delta -= (float)M_PI; // range -M_PI..M_PI
+                // Clamp to max rate
+                float deltaClamped = clamp(delta, -m_RotYSpeed*msgData.deltaSimTime, +m_RotYSpeed*msgData.deltaSimTime);
+                // Calculate new orientation, in a peculiar way in order to make sure the
+                // result gets close to m_orientation (rather than being n*2*M_PI out)
+                m_InterpolatedRotY = rotY + deltaClamped - delta;
+                recalculateXZRotation = true;
+            }
 
+            if (recalculateXZRotation)
+            {
+                m_LastRotX = m_RotX;
+                m_LastRotZ = m_RotZ;
+
+                UpdateXZRotation();
+
+                recalculateXZRotation = false;
+
+            }
+
             break;
         }
         case MT_TurnStart: