IK2D.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.
-- */

/*  IK2D.C                                                            */

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

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


#include "ik2d.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

static HWND      hParent;
static HINSTANCE hThisInstance;
static char *szViewClass="OFX:DraftClass";
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 int pen=0,stretched=1,tracetool=0;
static long Lv= -1,Fv= -1,Nv=0,Ne=0;
static struct TVLIST {BOOL inc; int x,y; long id; double l;
                      vector d; double theta;}  *TVlist=NULL;
static struct TELIST {BOOL inc; int v1,v2;}     *TElist=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 MoveVertex(HWND, int, int);
static void GetLengthAndAngles(void);
static BOOL TwoCross(vector, vector);
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 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,
        "2D 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);
      GetLengthAndAngles();
      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){
      MoveVertex(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)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;
 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].inc=TRUE;
   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 MoveVertex(HWND hWnd,int xi, int yi){
 double *J,*Jt,*dTheta,*I,*dI,*Ji;
 double l,dl,x[2],dx[2],dx_sum[2];
 double nx,ny,angle;
 long Nv1,i,j,k,n=0;
 if(Lv < 0)return;
 Nv1=Nv-1;
Nv1=Lv; 
 dx_sum[0]=dx_sum[1]=0.0;
 x[0]=dx[0]=(double)xi; x[1]=dx[1]=(double)yi;

#if 1
 x[0] = (double)TVlist[Nv1].x+dx[0]; x[1] = (double)TVlist[Nv1].y+dx[1]; //target
#endif

 if((dTheta=(double *)malloc((Nv1)*sizeof(double))) == NULL)return;
 if((J=(double *)malloc(2*(Nv1)*sizeof(double))) == NULL)return;
 if((Jt=(double *)malloc((Nv1)*2*sizeof(double))) == NULL)return;
 if((Ji=(double *)malloc((Nv1)*2*sizeof(double))) == NULL)return;
 if((dI=(double *)malloc(2*2*sizeof(double))) == NULL)return;
 if((I=(double *)malloc(2*2*sizeof(double))) == NULL)return;
 while(1){
   if(++n > 1000){/*MessageBox(NULL,"Out of iteration",NULL,MB_OK);*/ break;}
   CalculateJacobian(J,Nv1);
//J[3]=0.0;      // Lock
//J[Nv1+3]=0.0;  // Lock
//J[5]=0.0;      // Lock
//J[Nv1+5]=0.0;  // Lock
//for(i=0;i<Nv1;i++){
//J[i] *= (1.0+2.0*(double)(i)); // damping
//J[Nv1+i] *= (1.0+2.0*(double)(i)); // damping
//}
   CalculateTranspose(J,Jt,2,Nv1);
   MultiplyMatrix(J,2,Nv1,Jt,Nv1,2,I);
   if(InvertMatrix(I,2,dI)){
     MultiplyMatrix(Jt,Nv1,2,dI,2,2,Ji);
     if(!StepOK(J,Ji,dx,2,Nv1,0.01,1.e-6)){
       MessageBox(NULL,"Dx too small",NULL,MB_OK);
       break;                             // can't reach desired point
     }
     else{
       MultiplyMatrix(Ji,Nv1,2,dx,2,1,dTheta);

//for(i=0;i<Nv1;i++)dTheta[i] *= 1.0/(1.0+5.0*(double)(Nv1-1-i)); // damping old
//dTheta[2]=0.0; locking old

       for(i=0;i<Nv1;i++)TVlist[i].theta += dTheta[i];

#if 1
 nx=(double)TVlist[0].x;
 ny=(double)TVlist[0].y;
 angle=0.0;
 for(i=1;i<Nv;i++){
   angle += TVlist[i-1].theta;
   nx += TVlist[i-1].l * cos(angle);
   ny += TVlist[i-1].l * sin(angle);
   TVlist[i].x=(int)nx;
   TVlist[i].y=(int)ny;
 }
 dx_sum[0] = (double)TVlist[Nv1].x; dx_sum[1] = (double)TVlist[Nv1].y;
 dx[0]=x[0]-dx_sum[0]; dx[1]=x[1]-dx_sum[1];
#else
       dx_sum[0] += dx[0]; dx_sum[1] += dx[1];
#endif

       if(Dist(dx_sum,x,2) < 1.0)break;   // converged to solution
     }
   }
   else{
     MessageBeep(MB_OK);
   }
 }
 //
 // get new positions from the Theta
 //

#if 0
 nx=(double)TVlist[0].x;
 ny=(double)TVlist[0].y;
 angle=0.0;
 for(i=1;i<Nv;i++){
   angle += TVlist[i-1].theta;
   nx += TVlist[i-1].l * cos(angle);
   ny += TVlist[i-1].l * sin(angle);
   TVlist[i].x=(int)nx;
   TVlist[i].y=(int)ny;
 }
#endif

 // TVlist[Lv].x += xi; TVlist[Lv].y += yi;

 free(dTheta);
 free(Ji);
 free(I);
 free(dI);
 free(Jt);
 free(J);
 InvalidateRect(hWnd,NULL,FALSE);
 return;
}

static void CalculateJacobian(double *J, long n){
 long i,j,k;
 double sumx,sumy,angle;
 for(k=0;k<n;k++){
   sumx=sumy=0.0;
   for(i=k;i<n;i++){
     angle=0.0;
     for(j=0;j<=i;j++)angle += TVlist[j].theta;
     sumx += TVlist[i].l * sin(angle);
     sumy += TVlist[i].l * cos(angle);
   }
   J[k] = -sumx;
   J[n+k]=sumy;
 }
 return;
}

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 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 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 xInvertMatrix(double *I, long n, double *dI){
 double det;
 if(n != 2)return FALSE;
 det=I[0]*I[3] - I[2]*I[1];
 if(fabs(det) < 1.e-6)return FALSE;
 det = 1.0/det;
 dI[0] =  I[3]*det;
 dI[3] =  I[0]*det;
 dI[1] = -I[1]*det;
 dI[2] = -I[2]*det;
 return TRUE;
}

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;
}

static void GetLengthAndAngles(void){
 int i;
 FILE *f;
 vector v,x={1.0,0.0,0.0};
 double dx,dy,l,angle;
 if(Nv < 2)return;
 //f=fopen("f:\\debug","w");
 for(i=0;i<Nv-1;i++){
   dx=(double)(TVlist[i+1].x - TVlist[i].x);
   dy=(double)(TVlist[i+1].y - TVlist[i].y);
   TVlist[i].l=sqrt(dx*dx+dy*dy);
   TVlist[i].d[0]=dx/TVlist[i].l;
   TVlist[i].d[1]=dy/TVlist[i].l;
   TVlist[i].d[2]=0.0;
   if(i == 0)VECCOPY(x,v)
   else      VECCOPY(TVlist[i-1].d,v)
   if(TwoCross(v,TVlist[i].d)){
     angle=acos(DOT(TVlist[i].d,v));
     if(angle < 0.0)angle=PI-angle;
   }
   else{
     angle=acos(DOT(TVlist[i].d,v));
     if(angle < 0.0)angle=PI+angle;
     else angle=2*PI-angle;
   }
   TVlist[i].theta=angle;
   //fprintf(f,"Link %ld angle %lf\n",i,angle/PI*180.0);
 }
 //fclose(f);
}

static BOOL TwoCross(vector last, vector next){
 vector r;
 CROSS(last,next,r)
 if(r[2] < 0.0)return FALSE;
 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;
      if(hDLL == NULL)MessageBox ( GetFocus()," NULL instance",NULL, MB_OK);
      break;
    }
    case DLL_PROCESS_DETACH:
      break;
  }
  return TRUE;
}

#if __SC__
#pragma startaddress(DllMain)
#endif

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