reman3/Rayman_X/cpa/tempgrp/Owp/src/Circlway.c

697 lines
24 KiB
C
Raw Blame History

/*=========================================================================================
File: CirclWay.c
Prupose: Handle a CircleArc curve
Author: Yann Le Tensorer
Creation Date: 3 february 1997
Version: 1.1
Revision:
===========================================================================================
Revisions: Version Date Author
----------------------------------------------------------------
1.1 6 february 1997 Yann Le Tensorer
Added function "fn_vCircle_FindCenter"
=========================================================================================*/
/*==========================================================
Notes:
This module implements functions to create and draw CircleArc curves
here is an example how to use it:
{
tdstCircleArcObject CircleArcCurve;
long color;
MTH3D_tdstVector A,B,O;
ACP_tdstDynaParam stDynaParam;
unsigned char ucSamplingRate;
color=0xffffffff; // white
O.xX=0;
O.xY=0;
O.xZ=0
A.xX=0;
A.xY=1;
A.xZ=0;
B.xX=-1;
B.xY=0;
B.xZ=0;
ucSamplingRate=20; // Sampling rate
fn_vDynamicObject_Create(&stDynaParam,ucSamplingRate,C_ucSinus,0,MTH_C_Pi,255);
fn_vCircleArcObject_Create(&CircleArcCurve,ucSamplingRate,&A,&B,&O,C_ucModeNoObject,&stDynaParam);
fn_vCircleArcObject_SetColor(&CircleArcCurve,color);
fn_vCircleArcObject_SetViewPortAttributes(...); // en fonction du viewport
fn_vCircleArcObject_Draw(&CircleArcCurve);
fn_vCircleArcObject_Free(&CircleArcCurve);
}
===========================================================*/
#ifdef _AI_LIB_
#include "AIUseCPA.h"
/*#include "acp_base.h"*/
/*#include "gli.h"*/
#include "geoobj.h"
#include "circlway.h"
#include "IAOption.h"
#include "IAMacros.h"
/* AI memory and error management */
#include "MemIA.h"
#include "ErrIA.h"
#else /*_AI_LIB_*/
#include "acp_base.h"
#include "gli.h"
#include "geoobj.h"
#include "circlway.h"
#endif /*_AI_LIB_*/
/*============================================================
Memory management macros, to be replaced with correct memory management
============================================================*/
#ifdef _AI_LIB_
#define M_malloc(pointer,cast,size) M_IAAlloc(pointer,cast,size)
#define M_free(pointer) M_IAFree(pointer)
#else /*_AI_LIB_*/
#define M_malloc(pointer,cast,size) (pointer=(cast)malloc(size))
#define M_free(pointer) (free(pointer))
#endif /*_AI_LIB_*/
/*==========================================================
Function name: fn_vCircleArcObject_Create
Description: Create CircleArc curve (in ram only) between two points
Input: p_CircleArcObject: pointer to a preallocated tdstCircleArcObject
ucSamplingRate: number of segment to define the curve)
p_stStartPoint: pointer to the first vertex of the curve (in global repere)
p_stEndPoint: pointer to the last vertex of the curve (in global repere)
p_stCenter: pointer to the center of the circle (in global repere)
ucObjectMode: Creation mode (C_ucModeNoObject ou C_ucModeRealObject)
C_ucModeNoObject does not create a geometric object
whereas C_ucModeRealObject does.
Output: none
Author: Yann Le Tensorer
Date: 4 february 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_Create( tdstCircleArcObject *p_CircleArcObject,
unsigned char ucSamplingRate,
MTH3D_tdstVector* p_stStartPoint,
MTH3D_tdstVector* p_stEndPoint,
MTH3D_tdstVector* p_stCenter,
unsigned char ucObjectMode,
ACP_tdstDynaParam* p_stDynaParams
)
{
GEO_tdstDoubledIndex xLineDoubleIndex;
MTH3D_tdstVector TmpVector,TmpVector2,OA,OB,UnitOA,UnitOB;
MTH_tdxReal DotProduct;
long xCurVertex;
if (p_CircleArcObject==0) return; /* avoid crash */
/* copies input parameters into objects parameters */
p_CircleArcObject->ucObjectMode =ucObjectMode;
p_CircleArcObject->ucSamplingRate =ucSamplingRate;
p_CircleArcObject->stStartPoint =*p_stStartPoint;
p_CircleArcObject->stEndPoint =*p_stEndPoint;
p_CircleArcObject->stCenter =*p_stCenter;
p_CircleArcObject->p_stDynaParams =p_stDynaParams;
/* calculate OA and OB */
MTH3D_M_vSubVector(&OA,&p_CircleArcObject->stStartPoint,&p_CircleArcObject->stCenter);
MTH3D_M_vSubVector(&OB,&p_CircleArcObject->stEndPoint,&p_CircleArcObject->stCenter);
/*=====================================
calculates the local repere of the plan
=====================================*/
MTH3D_M_vNormalizeVector(&UnitOA,&OA);
MTH3D_M_vNormalizeVector(&UnitOB,&OB);
/* X axis = OA */
p_CircleArcObject->stLocalRepere.stCol_0=UnitOA;
/* Z axis = (OA^OB) */
MTH3D_M_vCrossProductVector(&TmpVector,&UnitOA,&UnitOB);
MTH3D_M_vNormalizeVector(&TmpVector,&TmpVector);
p_CircleArcObject->stLocalRepere.stCol_2=TmpVector;
/* Y axis = (OA^OB)^OA*/
MTH3D_M_vCrossProductVector(&TmpVector2,&TmpVector,&UnitOA);
MTH3D_M_vNormalizeVector(&TmpVector2,&TmpVector2);
p_CircleArcObject->stLocalRepere.stCol_1 = TmpVector2;
// MTH3D_M_vCrossProductVector(&p_CircleArcObject->stLocalRepere.stCol_1,&TmpVector,&UnitOA);
/*==================================================
Calculates inverse of local repere matrix
===================================================*/
MTH3D_M_vInverMatrix(&p_CircleArcObject->stInvLocalRepere,&p_CircleArcObject->stLocalRepere);
/*==================================================
calculates start and end coordinates in local repere
Xlocal(A)=inv(M)*(OA)
Xlocal(B)=inv(M)*(OB)
==================================================*/
fn_vCircleArcObject_GlobalToLocal( p_CircleArcObject,
&p_CircleArcObject->stLocalStartPoint,
&p_CircleArcObject->stStartPoint);
fn_vCircleArcObject_GlobalToLocal( p_CircleArcObject,
&p_CircleArcObject->stLocalEndPoint,
&p_CircleArcObject->stEndPoint);
/*==================================================
Calculates angle (OA,OB)
===================================================*/
DotProduct=MTH3D_M_xDotProductVector(&UnitOA,&UnitOB);
p_CircleArcObject->xMainAngle=MTH_M_xACos(DotProduct);
/* if mode is MODE_NO_OBJECT, allocates ram only for points list and dynamic parameters */
if (ucObjectMode==C_ucModeNoObject)
{
M_malloc ( p_CircleArcObject->d_stListOfPoints,
MTH3D_tdstVector*,
(ucSamplingRate+1)*sizeof(MTH3D_tdstVector)
);
}
else
/* else mode is MODE_REAL_OBJET, creates the geometric object*/
{
/* This list of points is not used */
p_CircleArcObject->d_stListOfPoints=0;
/* create object with 1 element (lines only), and ucSamplingRate+1 points) */
GEO_vCreateGeometricObject(&p_CircleArcObject->hObject,ucSamplingRate+1,1);
/* create line element*/
GEO_xCreateElementLines(p_CircleArcObject->hObject,&p_CircleArcObject->hElement,ucSamplingRate);
/* create lines indexes */
for (xCurVertex=0;xCurVertex<ucSamplingRate;xCurVertex++)
{
/* define start and end vertex of a line */
xLineDoubleIndex.a2_xIndex[0]=xCurVertex;
xLineDoubleIndex.a2_xIndex[1]=xCurVertex+1;
/* set line index */
GEO_xSetIndexOfElementLines(p_CircleArcObject->hObject,p_CircleArcObject->hElement,xCurVertex,&xLineDoubleIndex);
}
}
/* calculate points of CircleArc curve */
fn_vCircleArcObject_Calculate(p_CircleArcObject);
}
/*==========================================================
Function name: fn_vCircleArcObject_Free
Description: Free memory allocated by the CircleArc objet
Input: p_CircleArcObject: pointer to a tdstCircleArcObject
Output: none
Author: Yann Le Tensorer
Date: 29 january 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_Free(tdstCircleArcObject *p_CircleArcObject)
{
if (p_CircleArcObject!=0)
{
if (p_CircleArcObject->d_stListOfPoints!=0)
M_free(p_CircleArcObject->d_stListOfPoints);
/* if dynamic parameters are present, free memory too */
if (p_CircleArcObject->p_stDynaParams!=0)
fn_vDynamicObject_Free(p_CircleArcObject->p_stDynaParams);
}
}
/*==========================================================
Function name: fn_vCircleArcObject_Calculate
Description: Calculates the points of the b<>zier curve according to the
parameters specified when creating the object.
Input: p_CircleArcObject: Pointer to a pre-created CircleArc object
Output: none
Author: Yann Le Tensorer
Date: 5 february 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_Calculate(tdstCircleArcObject *p_CircleArcObject)
{
MTH_tdxReal xCurAngle,xDeltaAngle,xRayon;
MTH3D_tdstVector A,B,LocalCurPoint,OA;
MTH3D_tdstVector *p_stPointList;
int i;
if (p_CircleArcObject==0) return; /* avoid crash */
if (p_CircleArcObject->ucObjectMode==C_ucModeRealObject)
p_stPointList=((p_CircleArcObject->hObject) -> d_stListOfPoints);
else
p_stPointList=(p_CircleArcObject->d_stListOfPoints);
/* Calculates rayon of circle */
MTH3D_M_vSubVector(&OA,&p_CircleArcObject->stStartPoint,&p_CircleArcObject->stCenter);
xRayon=MTH3D_M_xNormVector(&OA);
A = p_CircleArcObject->stLocalStartPoint;
B = p_CircleArcObject->stLocalEndPoint;
xDeltaAngle=p_CircleArcObject->xMainAngle/p_CircleArcObject->ucSamplingRate;
for (i=0,xCurAngle=0;i<=p_CircleArcObject->ucSamplingRate;i++,xCurAngle+=xDeltaAngle)
{
LocalCurPoint.xX=MTH_M_xCos(xCurAngle)*xRayon;
LocalCurPoint.xY=MTH_M_xSin(xCurAngle)*xRayon;
LocalCurPoint.xZ=0;
fn_vCircleArcObject_LocalToGlobal(p_CircleArcObject,p_stPointList,&LocalCurPoint);
p_stPointList++;
}
}
/*==========================================================
Function name: fn_vCircleArcObject_SetColor
Description: Set the color of a CircleArc object.
Input: p_CircleArcObject: pointer to a pre-created tdstCircleArcObject
color: 24 bit color of object. (0->7=B; 8->15=G; 16->23=R ; 24->32=Unused)
Output: none
Author: Yann Le Tensorer
Date: 24 january 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_SetColor(tdstCircleArcObject *p_CircleArcObject,long color)
{
if (p_CircleArcObject==0) return; /* avoid crash */
p_CircleArcObject->color=color;
}
/*==========================================================
Function name: fn_vCircleArcObject_SetViewPortAttributes
Description: Set the viewportattributes for drawing.
Input: p_CircleArcObject: pointer to a pre-created tdstCircleArcObject
p_ViewPortAttributes: pointer to pre-defined viewport attributes
Output: none
Author: Yann Le Tensorer
Date: 24 january 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_SetViewPortAttributes(tdstCircleArcObject *p_CircleArcObject,GLD_tdstViewportAttributes *p_ViewPortAttributes)
{
if (p_CircleArcObject==0) return; /* avoid crash */
p_CircleArcObject->p_ViewPortAttributes=p_ViewPortAttributes;
}
/*==========================================================
Function name: fn_vCircleArcObject_Draw
Description: Draw CircleArc object on screen
Input: p_CircleArcObject: pointer to a pre-created tdstCircleArcObject
Output: none
Author: Yann Le Tensorer
Date: 24 january 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_Draw(tdstCircleArcObject *p_CircleArcObject)
{
POS_tdstCompletePosition stMatrix;
GEO_tdstColor ColBidon;
MTH3D_tdstVector *p_stPointList;
MTH3D_tdstVector *p_stPointListEnd;
POS_fn_vSetIdentityMatrix(&stMatrix);
if (p_CircleArcObject==0) return; /* avoid crash */
/*unsigned long color; */
ColBidon.xR=(float)0.5;
ColBidon.xG=(float)0.5;
ColBidon.xB=(float)0.5;
ColBidon.xA=(float)0.5;
if (p_CircleArcObject->ucObjectMode==C_ucModeRealObject)
p_stPointList=((p_CircleArcObject->hObject) -> d_stListOfPoints);
else
p_stPointList=(p_CircleArcObject->d_stListOfPoints);
if (p_stPointList==0) return;
GLI_xGetCameraMatrix(((GLI_tdstSpecificAttributesFor3D*)((p_CircleArcObject->p_ViewPortAttributes)->p_vSpecificToXD))->p_stCam,&stMatrix);
GLI_xLoadMatrix(&stMatrix);
GLI_vSetFog(100,100,&ColBidon);
p_stPointListEnd=p_stPointList+p_CircleArcObject->ucSamplingRate;
for (;p_stPointList<p_stPointListEnd;p_stPointList++)
{
GLI_xDraw3DLine16(
(struct GLD_tdstViewportAttributes_*)(p_CircleArcObject->p_ViewPortAttributes),
p_stPointList,
p_stPointList+1,
p_CircleArcObject->color);
}
GLI_xPopMatrix();
}
/*==========================================================
Function name: fn_vCircleArcObject_ChangeParams
Description: Changes the parameters of a CircleArc objet and recalculates the coordinates
of the points.
Input: p_CircleArcObject: Pointer to a pre-created CircleArc object
p_stStartPoint: Pointer to the start point
p_stEndPoint: Pointer to the end Point
p_stCenter: Pointer to the center of the circle
Output: none
Author: Yann Le Tensorer
Date: 6 february 1997
Revision:
Notes: If an input parameter is 0, this parameter is simply not
affected. This enables to change easily only one parameter
==========================================================*/
void fn_vCircleArcObject_ChangeParams(tdstCircleArcObject *p_CircleArcObject,
MTH3D_tdstVector* p_stStartPoint,
MTH3D_tdstVector* p_stEndPoint,
MTH3D_tdstVector* p_stCenter)
{
MTH3D_tdstVector TmpVector,TmpVector2,OA,OB,UnitOA,UnitOB;
MTH_tdxReal DotProduct;
if (p_CircleArcObject==0) return; /* avoid crash */
/* copies input parameters into objects parameters */
if (p_stStartPoint!=0) p_CircleArcObject->stStartPoint =*p_stStartPoint;
if (p_stEndPoint!=0) p_CircleArcObject->stEndPoint =*p_stEndPoint;
if (p_stCenter!=0) p_CircleArcObject->stCenter =*p_stCenter;
/* calculate OA and OB */
MTH3D_M_vSubVector(&OA,&p_CircleArcObject->stStartPoint,&p_CircleArcObject->stCenter);
MTH3D_M_vSubVector(&OB,&p_CircleArcObject->stEndPoint,&p_CircleArcObject->stCenter);
/*=====================================
calculates the local repere of the plan
=====================================*/
MTH3D_M_vNormalizeVector(&UnitOA,&OA);
MTH3D_M_vNormalizeVector(&UnitOB,&OB);
/* X axis = OA */
p_CircleArcObject->stLocalRepere.stCol_0=UnitOA;
/* Z axis = (OA^OB) */
MTH3D_M_vCrossProductVector(&TmpVector,&UnitOA,&UnitOB);
MTH3D_M_vNormalizeVector(&TmpVector,&TmpVector);
p_CircleArcObject->stLocalRepere.stCol_2=TmpVector;
/* Y axis = (OA^OB)^OA*/
MTH3D_M_vCrossProductVector(&TmpVector2,&TmpVector,&UnitOA);
MTH3D_M_vNormalizeVector(&TmpVector2,&TmpVector2);
p_CircleArcObject->stLocalRepere.stCol_1 = TmpVector2;
// MTH3D_M_vCrossProductVector(&p_CircleArcObject->stLocalRepere.stCol_1,&TmpVector,&UnitOA);
/*==================================================
Calculates inverse of local repere matrix
===================================================*/
MTH3D_M_vInverMatrix(&p_CircleArcObject->stInvLocalRepere,&p_CircleArcObject->stLocalRepere);
/*==================================================
calculates start and end coordinates in local repere
Xlocal(A)=inv(M)*(OA)
Xlocal(B)=inv(M)*(OB)
==================================================*/
fn_vCircleArcObject_GlobalToLocal( p_CircleArcObject,
&p_CircleArcObject->stLocalStartPoint,
&p_CircleArcObject->stStartPoint);
fn_vCircleArcObject_GlobalToLocal( p_CircleArcObject,
&p_CircleArcObject->stLocalEndPoint,
&p_CircleArcObject->stEndPoint);
/*==================================================
Calculates angle (OA,OB)
===================================================*/
DotProduct=MTH3D_M_xDotProductVector(&UnitOA,&UnitOB);
p_CircleArcObject->xMainAngle=MTH_M_xACos(DotProduct);
fn_vCircleArcObject_Calculate(p_CircleArcObject);
}
/*==========================================================
Function name: fn_xCircleArcObject_GetSpeed
Description: returns the speed at a given sample (n<> of the point) of a CircleArc object
Input: p_stDynaParam: pointer to a pre-created tdstDynaParam structure
ucSampleNo: n<> of the point to get the speed from.
Output: return value is the speed.
Author: Yann Le Tensorer
Date: 03 february 1997
Revision:
==========================================================*/
MTH_tdxReal fn_xCircleArcObject_GetSpeed(tdstCircleArcObject *p_CircleArcObject,unsigned char ucSampleNo)
{
if (p_CircleArcObject==0) return 0;
return fn_xDynamicObject_GetSpeed(p_CircleArcObject->p_stDynaParams,ucSampleNo);
}
/*==========================================================
Function name: fn_vCircleArcObject_GetPoint
Description: returns the coordinates in the global rep<65>re of the point at a
given sample (n<> of the point) of a CircleArc object.
Input: p_stDynaParam: pointer to a pre-created tdstDynaParam structure
ucSampleNo: n<> of the point to get the coordinates from.
p_ReturnVector: pointer to a vector to be returned.
Output: *p_ReturnVector is a vector countaining the.coordinates of the point.
Author: Yann Le Tensorer
Date: 03 february 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_GetPoint(tdstCircleArcObject *p_CircleArcObject,unsigned char ucSampleNo,MTH3D_tdstVector* p_ReturnVector)
{
MTH3D_tdstVector *p_stPointList;
if (p_CircleArcObject==0) return;
if (p_CircleArcObject->ucObjectMode==C_ucModeRealObject)
p_stPointList=((p_CircleArcObject->hObject) -> d_stListOfPoints);
else
p_stPointList=(p_CircleArcObject->d_stListOfPoints);
if (p_stPointList!=0)
*p_ReturnVector=p_stPointList[ucSampleNo];
}
/*==========================================================
Function name: fn_vCircleArcObject_GlobalToLocal
Description: Changes coordinates of a point from global coordinates to local.
Input: p_CircleArcObject: pointer to the circle arc object
p_stDestVector: pointer to the destination vector
p_stSourceVector: pointer to the source vector
Output: *p_stDestVector is a vector countaining the.coordinates in the local
repere of the plan.
Author: Yann Le Tensorer
Date: 4 february 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_GlobalToLocal( tdstCircleArcObject *p_CircleArcObject,
MTH3D_tdstVector *p_stDestVector,
MTH3D_tdstVector *p_stSourceVector)
{
MTH3D_M_vSubVector( &p_CircleArcObject->stTmpVector0,
p_stSourceVector,
&p_CircleArcObject->stCenter);
MTH3D_M_vMulMatrixVector( p_stDestVector,
&p_CircleArcObject->stInvLocalRepere,
&p_CircleArcObject->stTmpVector0);
}
/*==========================================================
Function name: fn_vCircleArcObject_LocalToGlobal
Description: Changes coordinates of a point from local coordinates to global
Input: p_CircleArcObject: pointer to the circle arc object
p_stDestVector: pointer to the destination vector
p_stSourceVector: pointer to the source vector
Output: *p_stDestVector is a vector countaining the.coordinates in the global
repere of the plan.
Author: Yann Le Tensorer
Date: 4 february 1997
Revision:
==========================================================*/
void fn_vCircleArcObject_LocalToGlobal( tdstCircleArcObject *p_CircleArcObject,
MTH3D_tdstVector *p_stDestVector,
MTH3D_tdstVector *p_stSourceVector)
{
MTH3D_M_vMulMatrixVector( p_stDestVector,
&p_CircleArcObject->stLocalRepere,
p_stSourceVector);
MTH3D_M_vAddVector( p_stDestVector,
p_stDestVector,
&p_CircleArcObject->stCenter);
}
/*==========================================================
Function name: fn_ucCircleArcObject_GetSamplingRate
Description: Gets the sampling rate of a CircleArcObject objet
Input: p_CircleArcObject: Pointer to a pre-created CircleArc object
Output: return value is the sampling rate of the circle arc object
Author: Yann Le Tensorer
Date: 5 february 1997
Revision:
==========================================================*/
unsigned char fn_ucCircleArcObject_GetSamplingRate(tdstCircleArcObject *p_CircleArcObject)
{
if (p_CircleArcObject==0)
return 0;
else
return p_CircleArcObject->ucSamplingRate;
}
/*===============================================================================
Function name: fn_bCircleArcObject_SetSamplingRate
Description: Modifies the sampling rate of a CircleArc object, and changes all
the depending parameters (including dynamic sampling rate)
Input: p_CircleArcObject: pointer to a preallocated tdstCircleArcObject
ucSamplingRate: New sampling rate
Output: return value is true if sampling rate could be modified,
false if not.
Author: Yann Le Tensorer
Date: 5 february 1997
Revision:
Note: The sampling rate can only be modified for objects that have been
created using C_ucModeNoObject at their creation.
================================================================================*/
ACP_tdxBool fn_bCircleArcObject_SetSamplingRate(tdstCircleArcObject *p_CircleArcObject,
unsigned char ucSamplingRate)
{
if (p_CircleArcObject==0) return FALSE;
if (p_CircleArcObject->ucObjectMode==C_ucModeNoObject)
{
/* copies new sampling rate value into objects parameters */
p_CircleArcObject->ucSamplingRate =ucSamplingRate;
if (p_CircleArcObject->ucSamplingRate<2) p_CircleArcObject->ucSamplingRate=2;
/* free old list of points and allocates new one */
if (p_CircleArcObject->d_stListOfPoints!=0)
M_free(p_CircleArcObject->d_stListOfPoints);
M_malloc ( p_CircleArcObject->d_stListOfPoints,
MTH3D_tdstVector*,
(p_CircleArcObject->ucSamplingRate+1)*sizeof(MTH3D_tdstVector)
);
/* calculate points of CircleArc curve */
fn_vCircleArcObject_Calculate(p_CircleArcObject);
/* checks if dynamic parameters are attached to the object
if yes, changes them to match the new sampling rate */
fn_vDynamicObject_ChangeSamplingRate(p_CircleArcObject->p_stDynaParams,
p_CircleArcObject->ucSamplingRate);
return TRUE;
}
else
return FALSE;
}
/*==========================================================
Function name: fn_vCircle_FindCenter
Description: Finds the 3D coordinate of the center of the circle on which are 3 points.
Input: p_FirstPoint: pointer to the first point
p_SecondPoint: pointer to the second point
p_ThirdPoint: pointer to the third point
p_ReturnCenter: pointer to a point to be returned.
Output: *p_ReturnCenter is a vector countaining the.coordinates of the center.
Author: Yann Le Tensorer
Date: 6 february 1997
Revision:
NOTE: The 3 points must have a z coordinate equal to zero! (In fact, the points must be
given in a local repere of the plan in which is the circle, where the x and y
axis define the plan) The Center of the screen is given in the local repere
of this plan, so with a zero coordinate too.
==========================================================*/
void fn_vCircle_FindCenter( MTH3D_tdstVector *p_FirstPoint,
MTH3D_tdstVector *p_SecondPoint,
MTH3D_tdstVector *p_ThirdPoint,
MTH3D_tdstVector *p_ReturnCenter)
{
MTH_tdxReal xa,xb,xc,ya,yb,yc,xm,xn,ym,yn,a,b,c,d,alpha,beta;
/* see technical notes of ways and waypoints specifications (ref.003ST037)
for explanations...*/
xa=p_FirstPoint->xX;
ya=p_FirstPoint->xY;
xb=p_SecondPoint->xX;
yb=p_SecondPoint->xY;
xc=p_ThirdPoint->xX;
yc=p_ThirdPoint->xY;
xm=MTH_M_xMul(MTH_M_xAdd(xa,xc),MTH_C_Inv2);
ym=MTH_M_xMul(MTH_M_xAdd(ya,yc),MTH_C_Inv2);
xn=MTH_M_xMul(MTH_M_xAdd(xb,xc),MTH_C_Inv2);
yn=MTH_M_xMul(MTH_M_xAdd(yb,yc),MTH_C_Inv2);
a=MTH_M_xSub(xm,xa);
b=MTH_M_xSub(ym,ya);
c=MTH_M_xSub(xn,xb);
d=MTH_M_xSub(yn,yb);
alpha=MTH_M_xAdd(MTH_M_xMul(a,xm),MTH_M_xMul(b,ym));
beta= MTH_M_xAdd(MTH_M_xMul(c,xn),MTH_M_xMul(d,yn));
p_ReturnCenter->xX=MTH_M_xDiv(
MTH_M_xSub(MTH_M_xMul(beta,b),MTH_M_xMul(alpha,d)),
MTH_M_xSub(MTH_M_xMul(b,c),MTH_M_xMul(a,d))
);
p_ReturnCenter->xY=MTH_M_xDiv(
MTH_M_xSub(MTH_M_xMul(alpha,c),MTH_M_xMul(beta,a)),
MTH_M_xSub(MTH_M_xMul(b,c),MTH_M_xMul(a,d))
);
p_ReturnCenter->xZ=MTH_C_ZERO;
}