IK3D.C

Go to the documentation of this file.

/* --
OpenFX version 2.0 - Modelling, Animation and Rendering Package
Copyright (C) 2000 - OpenFX Development Team

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

You may contact the OpenFX development team via elecronic mail
at core@openfx.org, or visit our website at http://openfx.org for
further information and support details.
-- */

/*  IK3D.C                                                            */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <windows.h>

#include "struct.h"
#include "dstruct.h"


#include "ik3d.h"

#define VECCOPY(a,b)    { b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; }
#define VECSUB(a,b,c)   { c[0]=a[0]-b[0]; c[1]=a[1]-b[1]; c[2]=a[2]-b[2];}
#define VECSUM(a,b,c)   { c[0]=a[0]+b[0]; c[1]=a[1]+b[1]; c[2]=a[2]+b[2];}
#define VECSCALE(a,b,c) { c[0]=(a)*b[0]; c[1]=(a)*b[1]; c[2]=(a)*b[2];}
#define DOT(a,b)        ( (a[0]*b[0]) + (a[1]*b[1]) + (a[2]*b[2]) )
#define CROSS(v1,v2,r)  { \
                          r[0] = (v1[1]*v2[2]) - (v2[1]*v1[2]);  \
                          r[1] = (v1[2]*v2[0]) - (v1[0]*v2[2]);  \
                          r[2] = (v1[0]*v2[1]) - (v2[0]*v1[1]);  \
                         }

#ifndef PI
#define PI 3.1415926
#endif

#ifndef PI2
#define PI2 PI/2
#endif

static HWND      hParent;
static HINSTANCE hThisInstance;
static char *szViewClass="OFX:IKtestClass";
static SYSTEM_INFO sys_info;
static int    xScreen,yScreen;
static DWORD fdwStyle = WS_POPUP | WS_CAPTION | ! WS_SYSMENU |
                        WS_MAXIMIZEBOX | WS_MINIMIZEBOX | WS_THICKFRAME;
static HCURSOR hcurSave,hcurPen1=NULL;
static char filename[256];

static int pen=0,tracetool=0,version=0;
static long Lv= -1,Fv= -1,Nv=0,Ne=0;
static struct TVLIST {BOOL inc; int x,y,z; long id;
                      vector p;
                      vector ax,ay,az;}     *TVlist=NULL;
static struct TELIST {BOOL inc; int v1,v2;} *TElist=NULL;
static FILE *debug=NULL;

static LRESULT CALLBACK MainWndProc(HWND, UINT, WPARAM, LPARAM);
static BOOL MenuCommand(HWND, WORD);
static BOOL Tester(void);
static void DrawRubberLine(HWND, int, int);
static void DrawList(HWND, HDC);
static void AddTraceVertex(int, int);
static int GetVertexAtCursor(int, int);
static void IterateTowardsGoal(HWND, int, int);
static void SetupStart(void);
static void RotateVectorRound(vector, vector, vector, double);
static BOOL normalize(vector v);
static void rotz(double [4][4], double);
static void roty(double [4][4], double);
static void rotx(double [4][4], double);
static void mv4by1(double [4][4], vector, vector);
static void m4by4(double [4][4], double [4][4], double [4][4]);
static void rotate_round_vector(double, vector, double [4][4]);
static void CalculateTranspose(double *, double *, long, long);
static void MultiplyMatrix(double *, long, long,
                           double *, long, long,
                           double *);
static BOOL InvertMatrix(double *, long, double *);
static BOOL StepOK(double *, double *, double *, long, long, double, double);
static void UpdateTheLinkage(long, double *);
static void CalculateJacobian(double *, long);
static double Length(double *, long);
static double Dist(double *, double *, long);

static BOOL Tester(void){
 MSG msg;
 HWND hDesktopWnd,hWnd;
 HDC hDCcaps;
 WNDCLASS wndclass;
 TVlist=NULL; TElist=NULL; Nv=0; Ne=0;
 hDesktopWnd = GetDesktopWindow();
 hDCcaps     = GetDC(hDesktopWnd);
 ReleaseDC(hDesktopWnd,hDCcaps);
 GetSystemInfo(&sys_info);
 wndclass.style         = 0;
 wndclass.lpfnWndProc   = (WNDPROC)MainWndProc;
 wndclass.cbClsExtra    = 0 ;
 wndclass.cbWndExtra    = 0 ;
 wndclass.hInstance     = hThisInstance ;
 wndclass.hIcon         = NULL;
 wndclass.hCursor       = LoadCursor (NULL, IDC_ARROW) ;
 wndclass.hbrBackground = GetStockObject (WHITE_BRUSH) ;
 wndclass.lpszMenuName  = "tracemenu" ;
 wndclass.lpszClassName = szViewClass;
 if (!RegisterClass (&wndclass))  return FALSE ;
 xScreen = GetSystemMetrics(SM_CXSCREEN) ;
 yScreen = GetSystemMetrics(SM_CYSCREEN) ;
 hWnd = CreateWindow(
        szViewClass,
        "Pseudo 3D Inverse Kinematics (IK) Test Engine",
        fdwStyle,
        25,
        25,
        xScreen-50,
        yScreen-50,
        hParent,
        NULL,
        hThisInstance,
        NULL) ;
 if(hWnd == NULL)return FALSE;
 hcurPen1=LoadCursor(hThisInstance,MAKEINTRESOURCE(IDC_PEN1));
 ShowWindow (hWnd,SW_SHOWNA);
 PostMessage(hWnd,WM_COMMAND,(WPARAM)IDM_DRAWIT,0);
 while(GetMessage (&msg,NULL,0,0)){
   TranslateMessage (&msg);
   DispatchMessage (&msg);
 }
 DestroyWindow(hWnd);
 if(hcurPen1 != NULL)DeleteObject(hcurPen1);
 UnregisterClass(wndclass.lpszClassName,wndclass.hInstance);
 if(TVlist != NULL)X__Free(TVlist); TVlist=NULL; Nv=0;
 if(TElist != NULL)X__Free(TElist); TElist=NULL; Ne=0;
 return TRUE;
}

LRESULT CALLBACK MainWndProc(HWND hWnd,UINT iMessage,WPARAM wParam,
                             LPARAM lParam){
 static BOOL bToolCaptured=FALSE;
 static HCURSOR hCursorSave;
 static POINT LastPt;
 PAINTSTRUCT ps;
 HDC hDC;
 int i;
 RECT rc;
 POINT pt;
 switch (iMessage) {
   case WM_DESTROY:
    break;
  case WM_COMMAND:
    return MenuCommand(hWnd,LOWORD(wParam));
    break;
  case WM_RBUTTONUP:
    if(!bToolCaptured)break;
    if(tracetool == 1){
      DrawRubberLine(hWnd,LOWORD(lParam),HIWORD(lParam));
      SetCursor(hCursorSave);
      ClipCursor(NULL);
      ReleaseCapture();
      bToolCaptured=FALSE;
      Lv= -1;
    }
    break;
  case WM_LBUTTONDOWN:
    if(bToolCaptured)break;
    if(tracetool == 1){
      bToolCaptured=TRUE;
      SetCapture(hWnd);
      hCursorSave=SetCursor(hcurPen1);
      GetClientRect(hWnd,&rc);
      pt.x=rc.left; pt.y=rc.top;
      ClientToScreen(hWnd,&pt);
      rc.left=pt.x; rc.top=pt.y;
      pt.x=rc.right; pt.y=rc.bottom;
      ClientToScreen(hWnd,&pt);
      rc.right=pt.x; rc.bottom=pt.y;
      ClipCursor(&rc);
    }
    else if(tracetool == 2){
      bToolCaptured=TRUE;
      SetCapture(hWnd);
      LastPt.x=LOWORD(lParam); LastPt.y=HIWORD(lParam);
      SetupStart();
      Lv=GetVertexAtCursor(LOWORD(lParam),HIWORD(lParam));
    }
    break;
  case WM_MOUSEMOVE:
    if(!bToolCaptured)break;
    if(tracetool == 1){
      DrawRubberLine(hWnd,(int)LastPt.x,(int)LastPt.y);
      LastPt.x=LOWORD(lParam); LastPt.y=HIWORD(lParam);
      DrawRubberLine(hWnd,(int)LastPt.x,(int)LastPt.y);
    }
    if(tracetool == 2){
      IterateTowardsGoal(hWnd,(int)LOWORD(lParam)-(int)LastPt.x,
                        (int)HIWORD(lParam)-(int)LastPt.y);
      LastPt.x=LOWORD(lParam); LastPt.y=HIWORD(lParam);
    }
    break;
  case WM_LBUTTONUP:
    if(!bToolCaptured)break;
    if(tracetool == 1){ /* add */
      LastPt.x=LOWORD(lParam); LastPt.y=HIWORD(lParam);
      if(Lv >= 0)DrawRubberLine(hWnd,(int)LastPt.x,(int)LastPt.y);
      AddTraceVertex((int)LOWORD(lParam),(int)HIWORD(lParam));
      DrawList(hWnd,NULL);
      DrawRubberLine(hWnd,(int)LastPt.x,(int)LastPt.y);
    }
    else if(tracetool == 2){ /* move */
      ReleaseCapture();
      bToolCaptured=FALSE;
    }
    break;
  case WM_PAINT:
    hDC = BeginPaint(hWnd, &ps);
    GetClientRect(hWnd,&rc);
    PatBlt(hDC,0,0,rc.right,rc.bottom,PATCOPY);
    DrawList(hWnd,hDC);
    EndPaint(hWnd,&ps);
    break ;
  case WM_SIZE:
    InvalidateRect(hWnd,NULL,FALSE);
    return 0;
  default:
      return DefWindowProc (hWnd, iMessage, wParam, lParam) ;
 }
 return 0L ;
}

BOOL MenuCommand(HWND hWnd, WORD id){
 int i;
 switch (id) {
   case IDM_PEN_RED  : pen=0; goto PENID;
   case IDM_PEN_WHITE: pen=1; goto PENID;
   case IDM_PEN_BLACK: pen=2; goto PENID;
   case IDM_PEN_CYAN : pen=3; goto PENID;
     PENID:
     CheckMenuItem(GetMenu(hWnd),IDM_PEN_RED,MF_UNCHECKED);
     CheckMenuItem(GetMenu(hWnd),IDM_PEN_WHITE,MF_UNCHECKED);
     CheckMenuItem(GetMenu(hWnd),IDM_PEN_BLACK,MF_UNCHECKED);
     CheckMenuItem(GetMenu(hWnd),IDM_PEN_CYAN,MF_UNCHECKED);
     if(pen == 0)CheckMenuItem(GetMenu(hWnd),IDM_PEN_RED,MF_CHECKED);
     if(pen == 1)CheckMenuItem(GetMenu(hWnd),IDM_PEN_WHITE,MF_CHECKED);
     if(pen == 2)CheckMenuItem(GetMenu(hWnd),IDM_PEN_BLACK,MF_CHECKED);
     if(pen == 3)CheckMenuItem(GetMenu(hWnd),IDM_PEN_CYAN,MF_CHECKED);
     DrawList(hWnd,NULL);
     break;
   case IDM_ADD:
     Lv= -1;
     EnableMenuItem(GetMenu(hWnd),IDM_ADD,MF_GRAYED);
     EnableMenuItem(GetMenu(hWnd),IDM_MOVE,MF_ENABLED);
     DrawMenuBar(hWnd);
     tracetool=1;
     break;
   case IDM_MOVE:
     Lv= -1;
     EnableMenuItem(GetMenu(hWnd),IDM_ADD,MF_ENABLED);
     EnableMenuItem(GetMenu(hWnd),IDM_MOVE,MF_GRAYED);
     DrawMenuBar(hWnd);
     tracetool=2;
     break;
   case IDM_EXIT:
     PostQuitMessage(0);
     break;
   default:
     break;
 }
 return TRUE;
}

static void DrawRubberLine(HWND hwnd, int x, int y){
 HPEN hOldPen,MyPen;
 int oldROP;
 HDC hDClocal;
 if(Lv < 0)return;
 hDClocal=GetDC(hwnd);
 if(pen == 0)MyPen=CreatePen(PS_SOLID,0,RGB(255,0,0));
 if(pen == 1)MyPen=CreatePen(PS_SOLID,0,RGB(0,0,255));
 if(pen == 2)MyPen=CreatePen(PS_SOLID,0,RGB(0,0,0));
 if(pen == 3)MyPen=CreatePen(PS_SOLID,0,RGB(0,255,0));
 hOldPen=SelectObject(hDClocal,MyPen);
 oldROP=SetROP2(hDClocal,R2_NOT);
 MoveToEx(hDClocal,x,y,NULL);
 LineTo(hDClocal,TVlist[Lv].x,TVlist[Lv].y);
 SetROP2(hDClocal,oldROP);
 SelectObject(hDClocal,hOldPen);
 DeleteObject(MyPen);
 ReleaseDC(hwnd,hDClocal);
}

static void DrawList(HWND hwnd, HDC hDC){
 int i,x,y;
 HPEN hOldPen,MyPen;
 HBRUSH hOldBrush;
 HDC hDClocal;
 if(hDC == NULL){
   hDClocal=GetDC(hwnd);
 }
 else hDClocal=hDC;
 if(pen == 0)MyPen=CreatePen(PS_SOLID,0,RGB(255,0,0));
 if(pen == 1)MyPen=CreatePen(PS_SOLID,0,RGB(0,0,255));
 if(pen == 2)MyPen=CreatePen(PS_SOLID,0,RGB(0,0,0));
 if(pen == 3)MyPen=CreatePen(PS_SOLID,0,RGB(0,255,0));
 hOldPen=SelectObject(hDClocal,MyPen);
 hOldBrush=SelectObject(hDClocal,GetStockObject(BLACK_BRUSH));
 if(Ne > 0 && TElist != NULL){
   for(i=0;i<Ne;i++){
     if(!TElist[i].inc)continue;
     x=TVlist[TElist[i].v1].x; y=TVlist[TElist[i].v1].y;
     MoveToEx(hDClocal,x,y,NULL);
     x=TVlist[TElist[i].v2].x; y=TVlist[TElist[i].v2].y;
     LineTo(hDClocal,x,y);
   }
 }
 if(Nv > 0 && TVlist != NULL){
   for(i=0;i<Nv;i++)if(TVlist[i].inc){
     x=TVlist[i].x; y=TVlist[i].y;
     MoveToEx(hDClocal,x,y,NULL);
     LineTo(hDClocal,x+(int)(TVlist[i].ax[0]*40),y+(int)(TVlist[i].ax[1]*40));
     MoveToEx(hDClocal,x,y,NULL);
     LineTo(hDClocal,x+(int)(TVlist[i].ay[0]*30),y+(int)(TVlist[i].ay[1]*30));
     Rectangle(hDClocal,
               TVlist[i].x-2,TVlist[i].y-2,
               TVlist[i].x+2,TVlist[i].y+2);
   }
 }
 SelectObject(hDClocal,hOldPen);
 SelectObject(hDClocal,hOldBrush);
 DeleteObject(MyPen);
 if(hDC == NULL){
   ReleaseDC(hwnd,hDClocal);
 }
}

static void AddTraceVertex(int x,int y){
 int i,id,found=0,flag=0;
 vector x0={1.0,0.0,0.0},y0={0.0,1.0,0.0},z0={0.0,0.0,1.0};
 struct TVLIST *v;
 struct TELIST *e;
 if(Nv > 0){
   for(i=0;i<Nv;i++){
     if(TVlist[i].inc && abs(x-TVlist[i].x) < 4 && abs(y-TVlist[i].y) < 4){
       found=1;
       if(Lv >= 0)flag=1;
       id=i;
     }
   }
 }
 if(!found){
   if(TVlist == NULL){
     if((v=(struct TVLIST *)X__Malloc(sizeof(struct TVLIST))) == NULL)return;
   }
   else{
     if((v=(struct TVLIST *)X__Realloc(TVlist,(Nv+1)*sizeof(struct TVLIST))) == NULL)return;
   }
   TVlist=v;
   v[Nv].x=x; v[Nv].y=y; v[Nv].z=0; v[Nv].inc=TRUE;
   v[Nv].p[0]=(double)x;
   v[Nv].p[1]=(double)y;
   v[Nv].p[2]=0.0;
   VECCOPY(x0,v[Nv].ax)
   VECCOPY(y0,v[Nv].ay)
   VECCOPY(z0,v[Nv].az)
   id=Nv;
   Nv++;
 }
 if(Lv < 0){
   Lv=id;
 }
 else{
   if(TElist == NULL){
     if((e=(struct TELIST *)X__Malloc(sizeof(struct TELIST))) == NULL)return;
   }
   else{
     if((e=(struct TELIST *)X__Realloc(TElist,(Ne+1)*sizeof(struct TELIST))) == NULL)return;
   }
   TElist=e;
   e[Ne].v1=id; e[Ne].v2=Lv; e[Ne].inc=TRUE;
   Lv=id;
   Ne++;
 }
 if(flag)Lv= -1;
}

static int GetVertexAtCursor(int x, int y){
 int i,id=Nv-1;
 if(Nv > 0)for(i=0;i<Nv;i++){
   if(TVlist[i].inc && abs(x-TVlist[i].x) < 4 && abs(y-TVlist[i].y) < 4)id=i;
 }
 return id;
}

static void IterateTowardsGoal(HWND hWnd,int xi, int yi){
 double *J,*Jt,*dTheta,*I,*dI,*Ji;
 double l,dl,x[3],dx[3],dx_sum[3];
 double;
 long Nv1,i,j,k,n=0;
 if(Lv < 0)return;
 Nv1=Nv-1;
 Nv1=Lv; 
// dx_sum[0]=dx_sum[1]=dx_sum[2]=0.0;
// x[0]=dx[0]=(double)xi;
// x[1]=dx[1]=(double)yi;
// x[2]=dx[2]=(double)0.0;
 x[0]=TVlist[Nv1].p[0]+(double)xi;  //
 x[1]=TVlist[Nv1].p[1]+(double)yi;  //
 x[2]=TVlist[Nv1].p[2]+(double)0.0; //
 VECSUB(x,TVlist[Nv1].p,dx) //
 if((dTheta=(double *)malloc((3*Nv1)*sizeof(double))) == NULL)return;
 if((J=(double *)malloc(3*(3*Nv1)*sizeof(double))) == NULL)return;
 if((Jt=(double *)malloc((3*Nv1)*3*sizeof(double))) == NULL)return;
 if((Ji=(double *)malloc((3*Nv1)*3*sizeof(double))) == NULL)return;
 if((dI=(double *)malloc(3*3*sizeof(double))) == NULL)return;
 if((I=(double *)malloc(3*3*sizeof(double))) == NULL)return;

 sprintf(filename,"f:\\ik3d%ld.dat",version); version++;
 //debug=fopen(filename,"w");
 while(1){
   VECSUB(x,TVlist[Nv1].p,dx) //
   if(++n > 200){MessageBox(NULL,"Out of iteration",NULL,MB_OK); break;}
   if(debug != NULL)fprintf(debug,"Iteration %ld\n",n);
   CalculateJacobian(J,Nv1);
   CalculateTranspose(J,Jt,3,3*Nv1);
   MultiplyMatrix(J,3,3*Nv1,Jt,3*Nv1,3,I);
   if(InvertMatrix(I,3,dI)){
     // calculate the generalised inverse of the jacobian J (i.e. Ji)
     MultiplyMatrix(Jt,3*Nv1,3,dI,3,3,Ji);
     // StepOK returns a reduced dx so that the product [J][Ji] is
     // close to the identity matrix [I] and therefore can be used to
     // determine incremental angular rotations.
     if(!StepOK(J,Ji,dx,3,3*Nv1,1.e-2,1.e-6)){
       MessageBox(NULL,"Dx too small",NULL,MB_OK);
       break;    // can't reach desired point
     }
     else{
       // calculate the incremental rotations
       MultiplyMatrix(Ji,3*Nv1,3,dx,3,1,dTheta);
       // determine the new position of the linkage
       UpdateTheLinkage(Nv1,dTheta);
//       dx_sum[0] += dx[0];  // how close to the goal the selected
//       dx_sum[1] += dx[1];  // end point of the linkage has got
//       dx_sum[2] += dx[2];
//       if(Dist(dx_sum,x,3) < 1.0){
       if(Dist(TVlist[Nv1].p,x,3) < 3.0){ //
         break;   // converged to solution
       }
     }
   }
   else{
     MessageBeep(MB_OK);
   }
 }

 if(debug != NULL)fclose(debug);
 free(dTheta);
 free(Ji);
 free(I);
 free(dI);
 free(Jt);
 free(J);
 InvalidateRect(hWnd,NULL,FALSE);
 return;
}

static void CalculateJacobian(double *J, // dimension 3 by n
                              long n     // number of entries
                                         // i.e. 1 less than points
                             ){
 long k,l;
 vector ax,ay,az,dp,Jx,Jy,Jz;
 for(l=0,k=0;k<n;k++){
   VECCOPY(TVlist[k].ax,ax)
   VECCOPY(TVlist[k].ay,ay)
   VECCOPY(TVlist[k].az,az)
   VECSUB(TVlist[n].p,TVlist[k].p,dp)
   CROSS(ax,dp,Jx)
   CROSS(ay,dp,Jy)
   CROSS(az,dp,Jz)
   J[l]       = Jx[0];
   J[l+n*3]   = Jx[1];
   J[l+n*3*2] = Jx[2];
   l++;
   J[l]       = Jy[0];
   J[l+n*3]   = Jy[1];
   J[l+n*3*2] = Jy[2];
   l++;
   J[l]       = Jz[0];
   J[l+n*3]   = Jz[1];
   J[l+n*3*2] = Jz[2];
   l++;
 }
 return;
}

static void SetupStart(void){
 int i;
 vector x0={1.0,0.0,0.0},y0={0.0,1.0,0.0},z0={0.0,0.0,1.0};
 if(Nv < 2)return;
 for(i=0;i<Nv;i++){
   TVlist[i].p[0]=(double)TVlist[i].x;
   TVlist[i].p[1]=(double)TVlist[i].y;
   TVlist[i].p[2]=0.0;
   VECCOPY(x0,TVlist[i].ax)
   VECCOPY(y0,TVlist[i].ay)
   VECCOPY(z0,TVlist[i].az)
 }
}

static void UpdateTheLinkage(long Nv1, double *dTheta){
 long i,j,l;
 double thetaX,thetaY,thetaZ;
 vector ax,ay,az,baseZero={0.0,0.0,0.0};
 for(i=0,l=0;i<Nv1;i++,l+=3){
   // extract the incremental rotations and axis vectors around which
   // they are to be applied.
   thetaX = *(dTheta+l);
   thetaY = *(dTheta+l+1);
   thetaZ = *(dTheta+l+2);
   VECCOPY(TVlist[i].ax,ax)
   VECCOPY(TVlist[i].ay,ay)
   VECCOPY(TVlist[i].az,az)
   // apply the rotation to all the vertex locations and base vectors
   // further down the linkage (including the end point)
   for(j=i+1;j<=Nv1;j++){
     RotateVectorRound(TVlist[j].p,TVlist[i].p,ax,thetaX);
     RotateVectorRound(TVlist[j].p,TVlist[i].p,ay,thetaY);
     RotateVectorRound(TVlist[j].p,TVlist[i].p,az,thetaZ);
     RotateVectorRound(TVlist[j].ax,baseZero,ax,thetaX);
     RotateVectorRound(TVlist[j].ax,baseZero,ay,thetaY);
     RotateVectorRound(TVlist[j].ax,baseZero,az,thetaZ);
//if(debug != NULL)fprintf(debug,"i=%ld j=%ld ax=(%lf,%lf,%lf)\n",i,j,
//TVlist[j].ax[0],TVlist[j].ax[1],TVlist[j].ax[2]);
     RotateVectorRound(TVlist[j].ay,baseZero,ax,thetaX);
     RotateVectorRound(TVlist[j].ay,baseZero,ay,thetaY);
     RotateVectorRound(TVlist[j].ay,baseZero,az,thetaZ);
     RotateVectorRound(TVlist[j].az,baseZero,ax,thetaX);
     RotateVectorRound(TVlist[j].az,baseZero,ay,thetaY);
     RotateVectorRound(TVlist[j].az,baseZero,az,thetaZ);
     TVlist[j].x=(int)TVlist[j].p[0];
     TVlist[j].y=(int)TVlist[j].p[1];
     TVlist[j].z=(int)TVlist[j].p[2]; // should be 0 for pseudo 2D
   }
 }
 return;
}

static BOOL StepOK(double *J, double *Ji, double *dx,
                   long n, long m,
                   double e, double dmin){
 BOOL result=TRUE;
 long i;
 double *I,*N,norm=0.0;
 if((I=(double *)malloc(n*n*sizeof(double))) == NULL)return FALSE;
 if((N=(double *)malloc(n*sizeof(double))) == NULL)return FALSE;
 MultiplyMatrix(J,n,m,Ji,m,n,I);
 for(i=0;i<n;i++){
   I[i*n+i] -= 1.0;
 }
 while(1){
   MultiplyMatrix(I,n,n,dx,n,1,N);
   norm=0.0;
   for(i=0;i<n;i++){
     norm += N[i] * N[i];
   }
   if(norm < e)break;
   for(i=0;i<n;i++)dx[i] *= 0.5;
   if(Length(dx,n) < dmin){
      result=FALSE;
      break;
   }
 }
 free(N);
 free(I);
 return result;
}

static void RotateVectorRound(vector v,
                              vector BaseOfRotation,
                              vector AxisOfRotation,
                              double theta){
 // modify the input vector to apply the rotational transformation
 double t[4][4];
 vector lv,lvv;
 VECSUB(v,BaseOfRotation,lv) // remove
 rotate_round_vector(theta,AxisOfRotation,t);  // get rotational transform
 mv4by1(t,lv,lvv);  // apply it
 VECSUM(lvv,BaseOfRotation,v)
 return;
}

static BOOL normalize(vector v){
 double n,nn;
 n= (double)((v[0]*v[0]) + (v[1]*v[1]) + (v[2]*v[2]));
 if(n < 1.e-10)return FALSE;
 nn=sqrt(n);
 n = 1.0 / nn;
 VECSCALE(n,v,v)
 return TRUE;
}


static void rotz(double tr[4][4], double ang){
 short i,j;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  if(i == j)tr[i][j]=1.0;
 }
  tr[0][0]= cos(ang);
  tr[0][1]=-sin(ang);
  tr[1][0]= sin(ang);
  tr[1][1]= cos(ang);
  return;
}

static void roty(double tr[4][4], double ang){
 short i,j;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  if(i == j)tr[i][j]=1.0;
 }
  tr[0][0]= cos(ang);
  tr[0][2]=-sin(ang);
  tr[2][0]= sin(ang);
  tr[2][2]= cos(ang);
  return;
}

static void rotx(double tr[4][4], double ang){
 long i,j;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  if(i == j)tr[i][j]=1.0;
 }
  tr[1][1]= cos(ang);
  tr[1][2]=-sin(ang);
  tr[2][1]= sin(ang);
  tr[2][2]= cos(ang);
  return;
}

static void mv4by1(double t4[4][4], vector v1, vector v2){
 double xx,yy,zz;
 xx=t4[0][0]*v1[0]+t4[0][1]*v1[1]+t4[0][2]*v1[2]+t4[0][3];
 yy=t4[1][0]*v1[0]+t4[1][1]*v1[1]+t4[1][2]*v1[2]+t4[1][3];
 zz=t4[2][0]*v1[0]+t4[2][1]*v1[1]+t4[2][2]*v1[2]+t4[2][3];
 v2[0]=xx; v2[1]=yy; v2[2]=zz;
 return;
}

static void m4by4(double t1[4][4], double t2[4][4], double tr[4][4]){
 short n=4,i,j,k;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  for(k=0;k<4;k++)tr[i][j]=tr[i][j]+t1[i][k]*t2[k][j];
 }
 return;
}

static void rotate_round_vector(double angle, vector v, double t[4][4]){
  double dx,dy,dz,phi,theta,dxy;
  double t1[4][4],t2[4][4],t3[4][4];
  dx=v[0]; dy=v[1]; dz=v[2];
  dxy=dx*dx+dy*dy;
  if(dxy < 0.00001){ /* vertical so just rotate about z  and return */
    if(dz > 0.0)rotz(t, angle);
    else        rotz(t,-angle);
    return;
  }
  dxy=sqrt(dxy);
  if     (dx == 0.0 && dy > 0.0)phi =  PI2;
  else if(dx == 0.0 && dy < 0.0)phi = -PI2;
  else phi = atan2(dy,dx);
  theta = atan2(dz,dxy);
  rotz(t3, -phi);
  roty(t2, -theta);
  m4by4(t2,t3,t1);
  rotx(t2, angle);
  m4by4(t2,t1,t3);
  roty(t2,theta);
  m4by4(t2,t3,t1);
  rotz(t2,phi);
  m4by4(t2,t1,t);
}

static double Length(double *dx, long n){
 long i;
 double l=0.0;
 for(i=0;i<n;i++)l += dx[i]*dx[i];
 return sqrt(l);
}

static double Dist(double *x, double *y, long n){
 long i;
 double d=0.0;
 for(i=0;i<n;i++)d += (x[i]-y[i])*(x[i]-y[i]);
 return sqrt(d);
}

static void CalculateTranspose(double *M, double *Mt, long n, long m){
 long i,j;                       // M  is "n" rows "m" columns  [n x m]
 for(i=0;i<n;i++){
   for(j=0;j<m;j++){
     *(Mt+j*n+i) = *(M+i*m+j);   // Mt[j*n+i] = M[i*m+j];
   }
 }
 return;
}

static void MultiplyMatrix(double *M1, long n1, long m1,
                           double *M2, long n2, long m2,
                           double *I){
 long   i,j,k,c;           // [n1 x m2][n2 x m2] result is [n1 x m2]
 double s;
 if(m1 != n2){MessageBox(NULL,"Bad Multiply",NULL,MB_OK); return;}
 c=n2;
 for(i=0;i<n1;i++){
   for(j=0;j<m2;j++){
     for(s=0.0,k=0;k<c;k++){
       s += (*(M1+i*m1+k)) * (*(M2+k*m2+j));
       //  s += M1[i*m1+k]*M2[k*m2+j];  //  M1[i][k] * M2[k][j]
     }
     *(I+i*m2+j) = s;                   //  I[i*m2+j]=s;   // I[i][j]
   }
 }
 return;
}

static BOOL InvertMatrix(double *A, long n, double *B){
 int i,j,k,imax,kp1;
 double del,amax,atmp,btmp,div,amult,eps=1.e-6;
 for(i=0;i<n;i++)for(j=0;j<n;j++){
   if(i == j)B[i*n+j]=1.0;
   else      B[i*n+j]=0.0;
 }
 del=1.0;
 for(k=0;k<n;k++){
   if(k < n-1){
     imax=k;
     amax=fabs(A[k*n+k]);
     kp1=k+1;
     for(i=kp1;i<n;i++){
       if(amax-fabs(A[i*n+k]) < 0.0){
         imax=i;
         amax=fabs(A[i*n+k]);
       }
     }
     if(imax != k){
       for(j=0;j<n;j++){
         atmp=A[imax*n+j];
         A[imax*n+j]=A[k*n+j];
         A[k*n+j]=atmp;
         btmp=B[imax*n+j];
         B[imax*n+j]=B[k*n+j];
         B[k*n+j]=btmp;
       }
       del = -del;
     }
   }
   if(fabs(A[k*n+k]) < eps)return FALSE;
   del=A[k*n+k]*del;
   div=1.0/A[k*n+k];
   for(j=0;j<n;j++){
     A[k*n+j]=A[k*n+j]*div;
     B[k*n+j]=B[k*n+j]*div;
   }
   for(i=0;i<n;i++){
     amult=A[i*n+k];
     if(i != k){
       for(j=0;j<n;j++){
         A[i*n+j]=A[i*n+j]-amult*A[k*n+j];
         B[i*n+j]=B[i*n+j]-amult*B[k*n+j];
       }
     }
   }
 }
 return TRUE;
}

#if __WATCOMC__
int APIENTRY LibMain(HANDLE hDLL, DWORD dwReason, LPVOID lpReserved){
#else
BOOL WINAPI DllMain(HANDLE hDLL, DWORD dwReason, LPVOID lpReserved){
#endif
  switch (dwReason) {
    case DLL_PROCESS_ATTACH: {
      hThisInstance=hDLL;
      break;
    }
    case DLL_PROCESS_DETACH:
      break;
  }
  return TRUE;
}

BOOL _Xmodeler
 (HWND parent_window,HWND info_window,X__STRUCTURE *lpevi){
 lpEVI=lpevi;
 tracetool=0;
 hParent=parent_window;
 return Tester();
}