Finish (?) adding fixed-point feature to warp.

src/warp.c

@@ -39,7 +39,7 @@ struct warpstuff {
     double fixedp[3];
     float rfixed;
     int has_fixed, fixed_coordsys;
-    float rfixeddisk;
+    float fixedrdisk;
     float rigidrot;
     double d2o[16], o2d[16];
     int has_d2o, has_o2d;
@@ -47,6 +47,7 @@ struct warpstuff {
 
     float tfrac;
     double realtime;
+    float fixertheta;
 
     struct speckcache slc[MAXDSPCTX];
 };
@@ -227,20 +228,22 @@ void windup( struct warpstuff *ws, Point *out, CONST Point *in )
    * theta(t/T0,r) = (t/T0)*T0*(r < rcore ? 1 : rcore/r)
    * So rotate in the XZ plane about (0,0,0) by theta(t/T0,r)
    */
-   float x = in->x[0];
-   float z = in->x[2];
-   float r = hypotf( x, z );
-   float omega =  (r<=ws->rcore) ? 1
-		: (r>=ws->routercore) ? ws->routercore/r
-		: slerp( 1+(r-ws->routercore)/(ws->routercore-ws->rcore),
-			1, ws->routercore/r );
-   float theta = omega * ws->tfrac + ws->rigidrot;
-   float s = fastsin2pi( theta );
-   float c = fastsin2pi( theta + 0.25 );
-
-   out->x[0] = x*c - z*s;
-   out->x[1] = in->x[1];
-   out->x[2] = x*s + z*c;
+    float x = in->x[0];
+    float z = in->x[2];
+    float r = hypotf( x, z );
+    float omega =  (r<=ws->rcoredisk) ? 1
+		: (r>=ws->routercoredisk) ? ws->routercoredisk/r
+		: slerp( 1+(r-ws->routercoredisk)/(ws->routercoredisk-ws->rcoredisk),
+			1, ws->routercoredisk/r );
+    float theta, s, c;
+
+    theta = omega * ws->tfrac + ws->rigidrot + ws->fixertheta;
+    s = fastsin2pi( theta );
+    c = fastsin2pi( theta + 0.25 );
+
+    out->x[0] = x*c - z*s;
+    out->x[1] = in->x[1];
+    out->x[2] = x*s + z*c;
 }
 
 void warpspecks( struct warpstuff *ws,
@@ -348,7 +351,7 @@ static void setup_coords( struct stuff *st, struct warpstuff *ws ) {
     ws->routercoredisk = rtodisk( st, ws, ws->routercore, ws->core_coordsys );
     switch(ws->has_fixed) {
     case 1:
-	ws->rfixeddisk = rtodisk( st, ws, ws->fixedp[0], ws->fixed_coordsys );
+	ws->fixedrdisk = rtodisk( st, ws, ws->fixedp[0], ws->fixed_coordsys );
 	break;
     case 3:
 	if(getT2d( fixed2d, st, ws, ws->fixed_coordsys )) {
@@ -385,8 +388,21 @@ warp_get_parti( struct dyndata *dd, struct stuff *st, double realtime )
   ws->tfrac = warpfrac(ws, realtime * 30);
   if(ws->has_zerotime)
     ws->tfrac -= warpfrac(ws, ws->zerotime);
-  if(ws->has_trange && ws->period0 != 0)
-    ws->tfrac *= (ws->tto - ws->tfrom) / ws->period0;
+   if(ws->has_trange && ws->period0 != 0)
+	ws->tfrac *= (ws->tto - ws->tfrom) / ws->period0;
+
+  if(ws->has_fixed) {
+    float fixedomega =
+      (ws->fixedrdisk <= ws->rcoredisk) ? 1
+      : (ws->fixedrdisk >= ws->routercoredisk) ?
+		    ws->routercoredisk / ws->fixedrdisk
+      : slerp( 1 + (ws->fixedrdisk - ws->routercoredisk)
+		    / (ws->routercoredisk - ws->rcoredisk),
+		 1, ws->routercoredisk / ws->fixedrdisk );
+    ws->fixertheta = -ws->tfrac * fixedomega;
+  } else {
+    ws->fixertheta = 0;
+  }
 
 
   if(getenv("WDBG")) printf("%g %g %g # rt frac frac(rt)\n", realtime, ws->tfrac, warpfrac(ws, realtime*30));