/*******************************************************************
GEODES/MANIFOLD.C        Manifold Computation        August 19, 1996

This routine computes several important n-dimensional manifolds.  It
computes the n-sphere, n-cylinder, n-ellipsoid, and n-torus. The
latter includes the n-sphere and n-ellipsoid as special cases.  This
routine computes the position vector,  Jacobian matrix, and Hessian
matrix for a given set of parameter values.  The results allow
computation of all tangent and curvature properties.  This includes
geodesic computation.

********************************************************************

The C language function prototypes are:
    void ntorus_init(int n, double *a, double *b);
    void ntorus_free();
    void ntorus(double *x, double *y, double *jacobian,
    double *hessian);

Arguments are:
    n ........ Dimension of parameter space x^i.
    a ........ Vector of ellipsoid radii.
    b ........ Vector of torus radii.
    x ........ Vector of parameter values x^i.
    y ........ Position vector in ambient rectangular coordinates.
    jacobian.. Jacobian matrix for x^i.
    hessian... Hessian matrix for x^i.

Initialization routines, like ntorus_init(), preset the dimension
and allocate work vectors to make the evaluation routine as
efficient as possible. After calculations are completed, the
ntorus_free() routine frees these work vectors.

Vector of ellipsoid radii a^r has dimension n+1.  For example, the
unit n-sphere is generated with all a[r]==1.0.  Vector of
torus radii b^i has dimension n.  However, the first element is not
used. 

********************************************************************

The ambient space dimension is n+1.

The n-dimensional sphere and torus are generated by a recurring
pattern in their algebraic representation. This pattern is described
on Internet page http://www.netcom.com/~elements/ntorus.html.

Reference:
    Manfredo do Carmo, Riemannian Geometry, Birkhauser, Boston, 1992

********************************************************************

This software can be modified or extended by:
    * Including other parametric representations.
    * Add localized salients (bumps) to the manifold.
    * Allow ambient space dimension greater than n+1.

Refer comments and questions to:
    William L. Anderson
    Elements Research
    7501 Windyrush Road
    Charlotte, NC 28226
    704-543-9180
    elements@ix.netcom.com
    http://www.netcom.com/~elements/

********************************************************************

Copyright (c) 1994-1996 Elements Research, William L. Anderson.

Permission to use, copy, modify, and distribute this software and
its documentation for any purpose without fee is hereby granted,
provided that the above copyright notice appear in all copies and
that both that copyright notice and this permission notice appear
in supporting documentation.

This software is provided "as is" without express or implied
warranty.

*******************************************************************/
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <malloc.h>

/* macros reduce multidimensional array to C language array */
#define SUBS2(n,i,j) ((i)*(n)+(j))
#define SUBS3(n,m,i,j,k) (((i)*(n)+(j))*(m)+(k))

static int n;
static int m;

static double *cos_x;
static double *sin_x;
static double *a;
static double *b;

/*******************************************************************
ntorus()       n-Torus, n-Ellipsoid, n-Sphere Computation

This function computes the position vector y, jacobian matrix, and
hessian matrix for a set of parameter x values.

The setup function ntorus_init() must be called first to set n,
and create working vectors. This limits the argument passing.  The
function ntorus_free() reverses the setup.

*******************************************************************/

void ntorus_init(int n_arg, double *a_arg, double *b_arg) {
int i,r;
n = n_arg;
m = n_arg + 1;
cos_x = malloc(sizeof(double)*n_arg);
sin_x = malloc(sizeof(double)*n_arg);
a = malloc(sizeof(double)*m);
for (r = 0; r < m; r++)
    a[r] = a_arg[r];
/* Allocated size of b is n+1. b[0] is not used.
b[n] must equal 0.0. This simplifies the evaluation
algorithm at the cost of a few more additions */
b = malloc(sizeof(double)*(n+1));
b[0] = 0.0;
for (i = 0; i < n; i++)
    b[i+1] = b_arg[i];
b[n] = 0.0;
}

void ntorus_free() {
n = 0;
m = 0;
free(cos_x);
free(sin_x);
free(a);
free(b);
cos_x = NULL;
sin_x = NULL;
a = NULL;
b = NULL;
}

void ntorus(double *x, double *y, double *jacobian, double *hessian) {
/* ntorus has nsphere, nellipsoid, and ncylinder as special cases */
int i,j,k,r;
for (i = 0; i < n; i++) {
    cos_x[i] = cos(x[i]);
    sin_x[i] = sin(x[i]);
    }
if (y != NULL)
    for (r = 0; r < m; r++) {
        y[r] = a[r];
        for (i = 0; i < r; i++) {
            y[r] *= sin_x[i];
            y[r] += b[i+1];
            }
        if (r < n)
            y[r] *= cos_x[i];
        }
if (jacobian != NULL)
    for (r = 0; r < m; r++) {
        for (j = 0; j < n; j++)
            if (j <= r)
                jacobian[SUBS2(n,r,j)] = a[r];
            else
                jacobian[SUBS2(n,r,j)] = 0.0;
        for (j = 0; j <= r && j < n; j++) {
            for (i = 0; i < r; i++)
                if (i != j) {
                    jacobian[SUBS2(n,r,j)] *= sin_x[i];
                    jacobian[SUBS2(n,r,j)] += b[i+1];
                    }
                else
                    jacobian[SUBS2(n,r,j)] *= cos_x[i];
            if (r < n)
                if (i != j)
                    jacobian[SUBS2(n,r,j)] *= cos_x[i];
                else
                    jacobian[SUBS2(n,r,j)] *= -sin_x[i];
            }
        }
if (hessian != NULL)
    for (r = 0; r < m; r++) {
        for (j = 0; j < n; j++)
            for (k = 0; k <= j; k++) {
                if (j <= r)
                    hessian[SUBS3(n,n,r,j,k)] = a[r];
                else
                    hessian[SUBS3(n,n,r,j,k)] = 0.0;
                if (k != j)
                    hessian[SUBS3(n,n,r,k,j)] = hessian[SUBS3(n,n,r,j,k)];
                }
        for (j = 0; j <= r && j < n; j++)
            for (k = 0; k <= j; k++) {
                for (i = 0; i < r; i++)
                    if (i != j && i != k) {
                        hessian[SUBS3(n,n,r,j,k)] *= sin_x[i];
                        hessian[SUBS3(n,n,r,j,k)] += b[i+1];
                        }
                    else if (i == j && i == k)
                        hessian[SUBS3(n,n,r,j,k)] *= -sin_x[i];
                    else
                        hessian[SUBS3(n,n,r,j,k)] *= cos_x[i];
                if (r < n)
                    if (i != j && i != k)
                        hessian[SUBS3(n,n,r,j,k)] *= cos_x[i];
                    else if (i == j && i == k)
                        hessian[SUBS3(n,n,r,j,k)] *= -cos_x[i];
                    else
                        hessian[SUBS3(n,n,r,j,k)] *= -sin_x[i];
                if (k != j)
                    hessian[SUBS3(n,n,r,k,j)] = 
hessian[SUBS3(n,n,r,j,k)];
                }
        }
}

/*******************************************************************
ncylinder()       n-Cylinder Computation

The recursive form of the n-ellipsoid makes linearizing the first
parameter simple. The resulting manifold is a cylinder with axis
along the y[0] axis. This function removes one factor from the
parametric representation.  It applies to n-ellipsoids, but not to
n-tori.

This function computes the position vector y, jacobian matrix, and
hessian matrix for a set of parameter x values.

The setup function ntorus_init() must be called first to set n,
and create working vectors. This limits argument passing.  The
function ntorus_free() reverses the setup.

*******************************************************************/

void ncylinder(double *x, double *y, double *jacobian, double *hessian) {
/* n-cylinder */
double a0_sin_x0;
int j,k,r;
ntorus(x,y,jacobian,hessian);
a0_sin_x0 = a[0] * sin_x[0];
if (y != NULL) {
    y[0] = x[0];
    for (r = 1; r < m; r++)
        y[r] /= a0_sin_x0;
    }
if (jacobian != NULL) {
    jacobian[SUBS2(n,0,0)] = 1.0;
    for (r = 1; r < m; r++)
        for (j = 0; j < n; j++)
            if (j != 0)
                jacobian[SUBS2(n,r,j)] /= a0_sin_x0;
            else
                jacobian[SUBS2(n,r,j)] = 0.0;
    }
if (hessian != NULL) {
    hessian[SUBS3(n,n,0,0,0)] = 0.0;
    for (r = 1; r < m; r++)
        for (j = 0; j < n; j++)
            for (k = 0; k <= j; k++) {
                if (j != 0 && k != 0)
                    hessian[SUBS3(n,n,r,j,k)] /= a0_sin_x0;
                else
                    hessian[SUBS3(n,n,r,j,k)] = 0.0;
                if (k != j)
                    hessian[SUBS3(n,n,r,k,j)] = hessian[SUBS3(n,n,r,j,k)];
                }
    }
}
