1 {
2 *****************************************************************************
3 * *
4 * This file is part of the ZCAD *
5 * *
6 * See the file COPYING.modifiedLGPL.txt, included in this distribution, *
7 * for details about the copyright. *
8 * *
9 * This program is distributed in the hope that it will be useful, *
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
12 * *
13 *****************************************************************************
14 }
15 {
16 @author(Andrey Zubarev <zamtmn@yandex.ru>)
17 }
18
19 unit uzegeometry;
20 {$INCLUDE def.inc}
21
22 interface
23 uses LCLProc,uzbtypesbase,uzbgeomtypes,uzbtypes, math;
24 resourcestring
25 rsDivByZero='Divide by zero';
26 const
27 RightAngle=pi/2;
28 EmptyMatrix: DMatrix4D = ((0, 0, 0, 0),
29 (0, 0, 0, 0),
30 (0, 0, 0, 0),
31 (0, 0, 0, 0));
32 OneMatrix: DMatrix4D = ((1, 0, 0, 0),
33 (0, 1, 0, 0),
34 (0, 0, 1, 0),
35 (0, 0, 0, 1));
36 TwoMatrix: DMatrix4D = ((2, 0, 0, 0),
37 (0, 2, 0, 0),
38 (0, 0, 2, 0),
39 (0, 0, 0, 2));
40 DefaultVP:IMatrix4=(0,0,
41 100,100);
42 IdentityQuaternion: GDBQuaternion = (ImagPart:(x:0;y:0;z:0); RealPart: 1);
43 EPSILON : Single = 1e-40;
44 EPSILON2 : Single = 1e-30;
45 eps=1e-14;
46 floateps=1e-6;
47 sqreps=1e-7;
48 bigeps=1e-10;
49 xAxisIndex=0;yAxisIndex=1;zAxisIndex=2;wAxisIndex=3;
50 ScaleOne:GDBVertex=(x:1;y:1;z:1);
51 OneVertex:GDBVertex=(x:1;y:1;z:1);
52 xy_Z_Vertex:GDBVertex=(x:0;y:0;z:1);
53 x_Y_zVertex:GDBVertex=(x:0;y:1;z:0);
54 _X_yzVertex:GDBVertex=(x:1;y:0;z:0);
55 MinusOneVertex:GDBVertex=(x:-1;y:-1;z:-1);
56 MinusInfinityVertex:GDBVertex=(x:NegInfinity;y:NegInfinity;z:NegInfinity);
57 InfinityVertex:GDBVertex=(x:Infinity;y:Infinity;z:Infinity);
58 NulVertex4D:GDBVertex4d=(x:0;y:0;z:0;w:1);
59 NulVector4D:DVector4D=(0,0,0,0);
60 NulVector4D2:DVector4D=(0,0,0,1);
61 NulVertex:GDBVertex=(x:0;y:0;z:0);
62 XWCS:GDBVertex=(x:1;y:0;z:0);
63 YWCS:GDBVertex=(x:0;y:1;z:0);
64 ZWCS:GDBVertex=(x:0;y:0;z:1);
65 XWCS4D:DVector4D=(1,0,0,1);
66 YWCS4D:DVector4D=(0,1,0,1);
67 ZWCS4D:DVector4D=(0,0,1,1);
68 NulVertex2D:GDBVertex2D=(x:0;y:0);
69 XWCS2D2D:GDBVertex2D=(x:1;y:0);
70 type Intercept3DProp=record
71 isintercept:GDBBoolean; //**< Есть это пересение или нет
72 interceptcoord:gdbvertex; //**< Точка пересечения X,Y,Z
73 t1,t2:GDBDouble; //**< позиция на линии 1 и 2 в виде относительных цифр от 0 до 1
74 end;
75 Intercept2DProp=record
76 isintercept:GDBBoolean;
77 interceptcoord:GDBvertex2D;
78 t1,t2:GDBDouble;
79 end;
80 DistAndPoint=record
81 point:gdbvertex;
82 d:GDBDouble;
83 end;
84 DistAndt=record
85 t,d:GDBDouble;
86 end;
87 TCSDir=(TCSDLeft,TCSDRight);
ToDVector4Fnull88 function ToDVector4F(const m:DVector4D):DVector4F;
ToDMatrix4Fnull89 function ToDMatrix4F(const m:DMatrix4D):DMatrix4F;
ToVertex2DInull90 function ToVertex2DI(const V:GDBVertex):GDBVertex2DI;
CrossVertexnull91 function CrossVertex(const Vector1, Vector2: GDBVertex): GDBVertex;inline;
VertexD2Snull92 function VertexD2S(const Vector1:GDBVertex): GDBVertex3S;inline;
intercept2dnull93 function intercept2d(const x1, y1, x2, y2, x3, y3, x4, y4: GDBDouble): GDBBoolean;inline;
intercept2d2null94 function intercept2d2(const x11, y11, x12, y12, x21, y21, x22, y22: GDBFloat): GDBBoolean;inline;
intercept2dmynull95 function intercept2dmy(const l1begin,l1end,l2begin,l2end:gdbvertex2d):intercept2dprop;//inline;
intercept3dmynull96 function intercept3dmy(const l1begin,l1end,l2begin,l2end:gdbvertex):intercept3dprop;inline;
intercept3dmy2null97 function intercept3dmy2(const l1begin,l1end,l2begin,l2end:gdbvertex):intercept3dprop;//inline;
98
99 //** Функция позволяет найти пересечение по 2-м координатам одной линии и другой
intercept3dnull100 function intercept3d(const l1begin,l1end,l2begin,l2end:gdbvertex):intercept3dprop;//inline;
101
102
pointinquad2dnull103 function pointinquad2d(const x1, y1, x2, y2, xp, yp: GDBFloat): GDBBoolean;inline;
104
105 //**Функция определения длины по двум точкам с учетом 3-х мерного пространства
Vertexlengthnull106 function Vertexlength(const Vector1, Vector2: GDBVertex): GDBDouble;{inline;}
107
SqrVertexlengthnull108 function SqrVertexlength(const Vector1, Vector2: GDBVertex): GDBDouble;inline;overload;
SqrVertexlengthnull109 function SqrVertexlength(const Vector1, Vector2: GDBVertex2d): GDBDouble;inline; overload;
110 //**нахождение точки смещения от одной точки к другой в зависимости от коэффициент а
Vertexmorphnull111 function Vertexmorph(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;inline;overload;
112 //**нахождение точки смещения от одной точки к другой в зависимости от коэффициент а
Vertexmorphnull113 function Vertexmorph(const Vector1, Vector2: GDBVertex2D; a: GDBDouble): GDBVertex2D;inline;overload;
114 //**нахождение точки смещения от одной точки к другой в зависимости от коэффициент а
VertexDmorphnull115 function VertexDmorph(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;overload;inline;
116 //**нахождение точки смещения от одной точки к другой в зависимости от коэффициент а
VertexDmorphnull117 function VertexDmorph(const Vector1, Vector2: GDBVertex3S; a: GDBDouble): GDBVertex3S;overload;inline;
Vertexanglenull118 function Vertexangle(const Vector1, Vector2: GDBVertex2d): GDBDouble;inline;
TwoVectorAnglenull119 function TwoVectorAngle(const Vector1, Vector2: GDBVertex): GDBDouble;inline;
oneVertexlengthnull120 function oneVertexlength(const Vector1: GDBVertex): GDBDouble;inline;
SqrOneVertexlengthnull121 function SqrOneVertexlength(const Vector1: GDBVertex): GDBDouble;inline;
vertexlen2dfnull122 function vertexlen2df(const x1, y1, x2, y2: GDBFloat): GDBFloat;inline;
NormalizeVertexnull123 function NormalizeVertex(const Vector1: GDBVertex): GDBVertex;{inline;}
VertexMulOnScnull124 function VertexMulOnSc(const Vector1:GDBVertex;sc:GDBDouble): GDBVertex;inline;
125
126 //к первой вершине прибавить вторую по осям Vector1.х + Vector2.х
VertexAddnull127 function VertexAdd(const Vector1, Vector2: GDBVertex): GDBVertex;inline;overload;
VertexAddnull128 function VertexAdd(const Vector1, Vector2: GDBVertex3S): GDBVertex3S;inline;overload;
Vertex2DAddnull129 function Vertex2DAdd(const Vector1, Vector2: GDBVertex2D): GDBVertex2D;inline;
VertexSubnull130 function VertexSub(const Vector1, Vector2: GDBVertex): GDBVertex;overload;inline;
VertexSubnull131 function VertexSub(const Vector1, Vector2: GDBvertex3S): GDBVertex3S;overload;inline;
MinusVertexnull132 function MinusVertex(const Vector1: GDBVertex): GDBVertex;inline;
vertexlen2idnull133 function vertexlen2id(const x1, y1, x2, y2: GDBInteger): GDBDouble;inline;
Vertexdmorphabsnull134 function Vertexdmorphabs(const Vector1, Vector2: GDBVertex;a: GDBDouble): GDBVertex;inline;
Vertexmorphabsnull135 function Vertexmorphabs(const Vector1, Vector2: GDBVertex;a: GDBDouble): GDBVertex;inline;
Vertexmorphabs2null136 function Vertexmorphabs2(const Vector1, Vector2: GDBVertex;a: GDBDouble): GDBVertex;inline;
MatrixMultiplynull137 function MatrixMultiply(const M1, M2: DMatrix4D):DMatrix4D;overload;inline;
MatrixMultiplynull138 function MatrixMultiply(const M1: DMatrix4D; M2: DMatrix4F):DMatrix4D;overload;inline;
MatrixMultiplyFnull139 function MatrixMultiplyF(const M1, M2: DMatrix4D):DMatrix4F;inline;
VectorTransformnull140 function VectorTransform(const V:GDBVertex4D;const M:DMatrix4D):GDBVertex4D;overload;inline;
VectorTransformnull141 function VectorTransform(const V:GDBVertex4D;const M:DMatrix4F):GDBVertex4D;overload;inline;
VectorTransformnull142 function VectorTransform(const V:GDBVertex4F;const M:DMatrix4F):GDBVertex4F;overload;inline;
143 procedure normalize4d(var tv:GDBVertex4d);overload;inline;
144 procedure normalize4F(var tv:GDBVertex4F);overload;inline;
VectorTransform3Dnull145 function VectorTransform3D(const V:GDBVertex;const M:DMatrix4D):GDBVertex;overload;inline;
VectorTransform3Dnull146 function VectorTransform3D(const V:GDBVertex;const M:DMatrix4F):GDBVertex;overload;inline;
VectorTransform3Dnull147 function VectorTransform3D(const V:GDBVertex3S;const M:DMatrix4D):GDBVertex3S;overload;inline;
VectorTransform3Dnull148 function VectorTransform3D(const V:GDBVertex3S;const M:DMatrix4F):GDBVertex3S;overload;inline;
149
FrustumTransformnull150 function FrustumTransform(const frustum:ClipArray;const M:DMatrix4D; MatrixAlreadyTransposed:Boolean=false):ClipArray;overload;inline;
FrustumTransformnull151 function FrustumTransform(const frustum:ClipArray;const M:DMatrix4F; MatrixAlreadyTransposed:Boolean=false):ClipArray;overload;inline;
152
153 procedure MatrixTranspose(var M: DMatrix4D);overload;inline;
154 procedure MatrixTranspose(var M: DMatrix4F);overload;inline;
155 procedure MatrixNormalize(var M: DMatrix4D);inline;
CreateRotationMatrixXnull156 function CreateRotationMatrixX(const Sine, Cosine: GDBDouble): DMatrix4D;inline;
CreateRotationMatrixYnull157 function CreateRotationMatrixY(const Sine, Cosine: GDBDouble): DMatrix4D;inline;
CreateRotationMatrixZnull158 function CreateRotationMatrixZ(const Sine, Cosine: GDBDouble): DMatrix4D;inline;
CreateRotatedXVectornull159 function CreateRotatedXVector(const angle: GDBDouble):GDBVertex;
CreateRotatedYVectornull160 function CreateRotatedYVector(const angle: GDBDouble):GDBVertex;
CreateAffineRotationMatrixnull161 function CreateAffineRotationMatrix(const anAxis: GDBvertex; angle: double):DMatrix4D;inline;
distance2piecenull162 function distance2piece(var q:GDBvertex2DI;var p1,p2:GDBvertex2D): double;overload;inline;
distance2piecenull163 function distance2piece(q:GDBvertex;var p1,p2:GDBvertex): {DistAndPoint}double;overload;//inline;
164
distance2piece_2null165 function distance2piece_2(var q:GDBvertex2DI; p1,p2:GDBvertex2D): double;overload;inline;
distance2piece_2null166 function distance2piece_2(var q:GDBvertex2DI; p1,p2:GDBvertex2DI): double;overload;inline;
distance2piece_2Dmynull167 function distance2piece_2Dmy(var q:GDBvertex2D; p1,p2:GDBvertex2D): double;inline;
168
distance2piece_2_xynull169 function distance2piece_2_xy(var q:GDBvertex2DI;const p1,p2:GDBvertex2D):GDBvertex2DI;inline;
170
distance2point_2null171 function distance2point_2(var p1,p2:GDBvertex2DI):GDBInteger;inline;
distance2raynull172 function distance2ray(q:GDBvertex;const p1,p2:GDBvertex):DistAndt;
CreateTranslationMatrixnull173 function CreateTranslationMatrix(const V:GDBvertex): DMatrix4D;inline;
CreateScaleMatrixnull174 function CreateScaleMatrix(const V:GDBvertex): DMatrix4D;inline;
CreateReflectionMatrixnull175 function CreateReflectionMatrix(plane:DVector4D): DMatrix4D;
176 //**Создать 3D вершину
CreateVertexnull177 function CreateVertex(const x,y,z:GDBDouble):GDBVertex;inline;
CreateVertexFromArraynull178 function CreateVertexFromArray(var counter:integer;const args:array of const):GDBVertex;
CreateDoubleFromArraynull179 function CreateDoubleFromArray(var counter:integer;const args:array of const):GDBDouble;
CreateGDBStringFromArraynull180 function CreateGDBStringFromArray(var counter:integer;const args:array of const):GDBString;
181 //**Создать 2D вершину
CreateVertex2Dnull182 function CreateVertex2D(const x,y:GDBDouble):GDBVertex2D;inline;
IsPointInBBnull183 function IsPointInBB(const point:GDBvertex; var fistbb:TBoundingBox):GDBBoolean;inline;
CreateBBFrom2Pointnull184 function CreateBBFrom2Point(const p1,p2:GDBvertex):TBoundingBox;
CreateBBFromPointnull185 function CreateBBFromPoint(const p:GDBvertex):TBoundingBox;
186 procedure ConcatBB(var fistbb:TBoundingBox;const secbb:TBoundingBox);inline;
187 procedure concatBBandPoint(var fistbb:TBoundingBox;const point:GDBvertex);inline;
IsBBNulnull188 function IsBBNul(const bb:TBoundingBox):boolean;inline;
boundingintersectnull189 function boundingintersect(const bb1,bb2:TBoundingBox):GDBBoolean;inline;
190 procedure MatrixInvert(var M: DMatrix4D);//inline;
vectordotnull191 function vectordot(const v1,v2:GDBVertex):GDBVertex;inline;
scalardotnull192 function scalardot(const v1,v2:GDBVertex):GDBDouble;//inline;
vertexeqnull193 function vertexeq(const v1,v2:gdbvertex):GDBBoolean;inline;
SQRdist_Point_to_Segmentnull194 function SQRdist_Point_to_Segment(const p:GDBVertex;const s0,s1:GDBvertex):gdbdouble;inline;
NearestPointOnSegmentnull195 function NearestPointOnSegment(const p:GDBVertex;const s0,s1:GDBvertex):GDBvertex;inline;
IsPointEqualnull196 function IsPointEqual(const p1,p2:gdbvertex):boolean;inline;
IsVectorNulnull197 function IsVectorNul(const p2:gdbvertex):boolean;inline;
IsDoubleNotEqualnull198 function IsDoubleNotEqual(const d1,d2:gdbdouble;const _eps:gdbdouble=eps):boolean;inline;
IsFloatNotEqualnull199 function IsFloatNotEqual(const d1,d2:gdbfloat;const _floateps:gdbfloat=floateps):boolean;inline;
200
201 procedure _myGluProject(const objx,objy,objz:GDBdouble;const modelMatrix,projMatrix:PDMatrix4D;const viewport:PIMatrix4; out winx,winy,winz:GDBdouble);inline;
202 procedure _myGluProject2(const objcoord:GDBVertex;const modelMatrix,projMatrix:PDMatrix4D;const viewport:PIMatrix4; out wincoord:GDBVertex);inline;
203 procedure _myGluUnProject(const winx,winy,winz:GDBdouble;const modelMatrix,projMatrix:PDMatrix4D;const viewport:PIMatrix4;out objx,objy,objz:GDBdouble);inline;
204
orthonull205 function ortho(const xmin,xmax,ymin,ymax,zmin,zmax:GDBDouble;const matrix:PDMatrix4D):DMatrix4D;{inline;}
Perspectivenull206 function Perspective(const fovy,W_H,zmin,zmax:GDBDouble;const matrix:PDMatrix4D):DMatrix4D;inline;
LookAtnull207 function LookAt(point,ex,ey,ez:GDBvertex;const matrix:PDMatrix4D):DMatrix4D;inline;
208
calcfrustumnull209 function calcfrustum(const clip:PDMatrix4D):cliparray;inline;
PointOf3PlaneIntersectnull210 function PointOf3PlaneIntersect(const P1,P2,P3:DVector4D):GDBVertex;inline;
PointOfLinePlaneIntersectnull211 function PointOfLinePlaneIntersect(const p1,d:GDBVertex;const plane:DVector4D;out point :GDBVertex):GDBBoolean;{inline;}
PlaneFrom3Pontnull212 function PlaneFrom3Pont(const P1,P2,P3:GDBVertex):DVector4D;inline;
213 procedure NormalizePlane(var plane:DVector4D);{inline;}
214
CalcTrueInFrustumnull215 function CalcTrueInFrustum (const lbegin,lend:GDBvertex; const frustum:ClipArray):TInBoundingVolume;overload;//inline;
CalcTrueInFrustumnull216 function CalcTrueInFrustum (const lbegin,lend:GDBvertex3S; const frustum:ClipArray):TInBoundingVolume;overload;
CalcPointTrueInFrustumnull217 function CalcPointTrueInFrustum (const lbegin:GDBvertex; const frustum:ClipArray):TInBoundingVolume;
CalcOutBound4VInFrustumnull218 function CalcOutBound4VInFrustum (const OutBound:OutBound4V; const frustum:ClipArray):TInBoundingVolume;inline;
CalcAABBInFrustumnull219 function CalcAABBInFrustum (const AABB:TBoundingBox; const frustum:ClipArray):TInBoundingVolume;{inline;}
220
GetXfFromZnull221 function GetXfFromZ(oz:GDBVertex):GDBVertex;
222
MatrixDeterminantnull223 function MatrixDeterminant(M: DMatrix4D): GDBDouble;
CreateMatrixFromBasisnull224 function CreateMatrixFromBasis(const ox,oy,oz:GDBVertex):DMatrix4D;
225 procedure CreateBasisFromMatrix(const m:DMatrix4D;out ox,oy,oz:GDBVertex);
226
QuaternionFromMatrixnull227 function QuaternionFromMatrix(const mat : DMatrix4D) : GDBQuaternion;
QuaternionSlerpnull228 function QuaternionSlerp(const source, dest: GDBQuaternion; const t: GDBDouble): GDBQuaternion;
QuaternionToMatrixnull229 function QuaternionToMatrix(quat : GDBQuaternion) : DMatrix4D;
230
GetArcParamFrom3Point2Dnull231 function GetArcParamFrom3Point2D(Const PointData:tarcrtmodify;out ad:TArcData):GDBBoolean;
232
isNotReadableAnglenull233 function isNotReadableAngle(Angle:GDBDouble):GDBBoolean;
CorrectAngleIfNotReadablenull234 function CorrectAngleIfNotReadable(Angle:GDBDouble):GDBDouble;
235
GetCSDirFrom0x0y2Dnull236 function GetCSDirFrom0x0y2D(const ox,oy:GDBVertex):TCSDir;
237
CalcDisplaySubFrustumnull238 function CalcDisplaySubFrustum(const x,y,w,h:gdbdouble;const mm,pm:DMatrix4D;const vp:IMatrix4):ClipArray;
myPickMatrixnull239 function myPickMatrix(const x,y,deltax,deltay:gdbdouble;const vp:IMatrix4): DMatrix4D;
240
241 var WorldMatrix{,CurrentCS}:DMatrix4D;
242 wx:PGDBVertex;
243 wy:PGDBVertex;
244 wz:PGDBVertex;
245 w0:PGDBVertex;
246 type
247 TLineClipArray=array[0..5]of gdbdouble;
248 implementation
ToDVector4Fnull249 function ToDVector4F(const m:DVector4D):DVector4F;
250 begin
251 result[0]:=m[0];
252 result[1]:=m[1];
253 result[2]:=m[2];
254 result[3]:=m[3];
255 end;
ToDMatrix4Fnull256 function ToDMatrix4F(const m:DMatrix4D):DMatrix4F;
257 begin
258 result[0]:=ToDVector4F(m[0]);
259 result[1]:=ToDVector4F(m[1]);
260 result[2]:=ToDVector4F(m[2]);
261 result[3]:=ToDVector4F(m[3]);
262 end;
ToVertex2DInull263 function ToVertex2DI(const V:GDBVertex):GDBVertex2DI;
264 begin
265 result.x:=round(V.x);
266 result.y:=round(V.y);
267 end;
myPickMatrixnull268 function myPickMatrix(const x,y,deltax,deltay:gdbdouble;const vp:IMatrix4): DMatrix4D;
269 var
270 tm,sm: DMatrix4D;
271 begin
272 tm:=CreateTranslationMatrix(createvertex((vp[2]-2*(x-vp[0]))/deltax,
273 (vp[3]-2*(y-vp[1]))/deltay, 0));
274 sm:=CreateScaleMatrix(createvertex(vp[2]/deltax,vp[3]/deltay, 1.0));
275 result:=MatrixMultiply(sm,tm);
276 end;
277
278 (*
279 {
280 if (deltax <= 0 || deltay <= 0) {
281 return;
282 }
283
284 /* Translate and scale the picked region to the entire window */
285 glTranslatef((viewport[2] - 2 * (x - viewport[0])) / deltax,
286 (viewport[3] - 2 * (y - viewport[1])) / deltay, 0);
287 glScalef(viewport[2] / deltax, viewport[3] / deltay, 1.0);
288 }
289 *)
CalcDisplaySubFrustumnull290 function CalcDisplaySubFrustum(const x,y,w,h:gdbdouble;const mm,pm:DMatrix4D;const vp:IMatrix4):ClipArray;
291 var
292 tm: DMatrix4D;
293 begin
294 (*//use glu.gluPickMatrix
295 oglsm.myglMatrixMode(GL_Projection);
296 oglsm.myglpushmatrix;
297 glLoadIdentity;
298 gluPickMatrix(x,y,w,h,{$IFNDEF DELPHI}PTViewPortArray(@vp)^{$ELSE}TVector4i(vp){$ENDIF});
299 glGetDoublev(GL_PROJECTION_MATRIX, @tm);
300 tm := MatrixMultiply(pm, tm);
301 tm := MatrixMultiply(mm, tm);
302 result := calcfrustum(@tm);
303 oglsm.myglpopmatrix;
304 oglsm.myglMatrixMode(GL_MODELVIEW);
305 *)
306 //use glu.gluPickMatrix
307 tm := myPickMatrix(x,y,w,h,vp);
308 tm := MatrixMultiply(pm, tm);
309 tm := MatrixMultiply(mm, tm);
310 result := calcfrustum(@tm);
311 end;
VertexD2Snull312 function VertexD2S(const Vector1:GDBVertex): GDBVertex3S;
313 begin
314 result.x:=Vector1.x;
315 result.y:=Vector1.y;
316 result.z:=Vector1.z;
317 end;
GetCSDirFrom0x0y2Dnull318 function GetCSDirFrom0x0y2D(const ox,oy:GDBVertex):TCSDir;
319 begin
320 if vectordot(ox,oy).z>eps then
321 result:=TCSDLeft
322 else
323 result:=TCSDRight;
324 end;
325
isNotReadableAnglenull326 function isNotReadableAngle(Angle:GDBDouble):GDBBoolean;
327 begin
328 if (Angle>(pi*0.5+eps))and(Angle<(pi*1.5+eps)) then
329 result:=true
330 else
331 result:=false;
332 end;
CorrectAngleIfNotReadablenull333 function CorrectAngleIfNotReadable(Angle:GDBDouble):GDBDouble;
334 begin
335 if isNotReadableAngle(Angle) then
336 result:=angle-pi
337 else
338 result:=angle;
339 end;
340
GetXfFromZnull341 function GetXfFromZ(oz:GDBVertex):GDBVertex;
342 begin
343 if (abs (oz.x) < 1/64) and (abs (oz.y) < 1/64) then
344 result:=CrossVertex(YWCS,oz)
345 else
346 result:=CrossVertex(ZWCS,oz);
347 normalizevertex(oz);
348 end;
349
IsPointEqualnull350 function IsPointEqual(const p1,p2:gdbvertex):boolean;
351 begin
352 if SqrVertexlength(p1,p2)>sqreps then
353 result:=false
354 else
355 result:=true;
356
357 end;
IsVectorNulnull358 function IsVectorNul(const p2:gdbvertex):boolean;
359 begin
360 if SqrOneVertexlength(p2)>sqreps then
361 result:=false
362 else
363 result:=true;
364 end;
IsDoubleNotEqualnull365 function IsDoubleNotEqual(const d1,d2:gdbdouble;const _eps:gdbdouble=eps):boolean;
366 begin
367 if abs(d1-d2)>_eps then
368 result:=true
369 else
370 result:=false;
371 end;
IsFloatNotEqualnull372 function IsFloatNotEqual(const d1,d2:gdbfloat;const _floateps:gdbfloat=floateps):boolean;
373 begin
374 if abs(d1-d2)>_floateps then
375 result:=true
376 else
377 result:=false;
378 end;
GetMinAndSwapnull379 function GetMinAndSwap(var position:integer;size:integer;var ca:TLineClipArray):gdbdouble;
380 var i,hpos:GDBInteger;
381 d1{,d2}:gdbdouble;
382 // bytebegin,byteend,bit:integer;
383 // cacount:integer;
384 // d:gdbvertex;
385 begin
386 result:=ca[position];
387 hpos:=-1;
388 for I := position to size do
389 begin
390 if ca[i]<=result then
391 begin
392 result:=ca[i];
393 hpos:=i;
394 end;
395 end;
396 if hpos<>-1 then
397 begin
398 d1:=ca[hpos];
399 ca[hpos]:=ca[position];
400 ca[position]:=d1;
401 end;
402 result:=ca[position];
403 inc(position);
404 end;
CalcOutBound4VInFrustumnull405 function CalcOutBound4VInFrustum (const OutBound:OutBound4V; const frustum:ClipArray):TInBoundingVolume;
406 var i,count:GDBInteger;
407 d1,d2,d3,d4:gdbdouble;
408 begin
409 count:=0;
410 for i:=0 to 5 do
411 begin
412 d1:=frustum[i][0] * outbound[0].x + frustum[i][1] * outbound[0].y + frustum[i][2] * outbound[0].z + frustum[i][3];
413 d2:=frustum[i][0] * outbound[1].x + frustum[i][1] * outbound[1].y + frustum[i][2] * outbound[1].z + frustum[i][3];
414 d3:=frustum[i][0] * outbound[2].x + frustum[i][1] * outbound[2].y + frustum[i][2] * outbound[2].z + frustum[i][3];
415 d4:=frustum[i][0] * outbound[3].x + frustum[i][1] * outbound[3].y + frustum[i][2] * outbound[3].z + frustum[i][3];
416 if (d1<0)and(d2<0)and(d3<0)and(d4<0)
417 then
418 begin
419 result:=irempty;
420 system.exit;
421 end;
422 if d1>=0 then inc(count);
423 if d2>=0 then inc(count);
424 if d3>=0 then inc(count);
425 if d4>=0 then inc(count);
426 end;
427 if count=24 then
428 begin
429 result:=irfully;
430 exit;
431 end;
432
433 result:=IRPartially;
434 end;
CalcTrueInFrustumnull435 function CalcTrueInFrustum (const lbegin,lend:GDBvertex; const frustum:ClipArray):TInBoundingVolume;
436 var i,j:GDBInteger;
437 d1,d2:gdbdouble;
438 bytebegin,byteend,bit:integer;
439 ca:TLineClipArray;
440 cacount:integer;
441 d,p:gdbvertex;
442 begin
443 fillchar((@ca)^,sizeof(ca),0);
444 result:=IREmpty;
445 bit:=1;
446 bytebegin:=0;
447 byteend:=0;
448 cacount:=0;
449 for i:=0 to 5 do
450 begin
451 d1:=frustum[i][0] * lbegin.x + frustum[i][1] * lbegin.y + frustum[i][2] * lbegin.z + frustum[i][3];
452 d2:=frustum[i][0] * lend.x + frustum[i][1] * lend.y + frustum[i][2] * lend.z + frustum[i][3];
453 if d1<0 then
454 bytebegin:=bytebegin or bit;
455 if d2<0 then
456 byteend:=byteend or bit;
457 if ((bytebegin and bit)and(byteend and bit))>0 then
458 begin
459 result:=IREmpty;
460 exit;
461 end;
462 if((bytebegin and bit)xor(byteend and bit))>0then
463 begin
464 d1:=abs(d1);
465 d2:=abs(d2);
466 ca[cacount]:=d1/(d1+d2);
467 inc(cacount);
468 end;
469 bit:=bit*2;
470 end;
471 if ((bytebegin)=0)and((byteend)=0) then
472 begin
473 result:=IRFully;
474 exit;
475 end;
476
477 if (bytebegin)=0 then
478 begin
479 result:=IRPartially;
480 exit;
481 end;
482 if (byteend)=0 then
483 begin
484 result:=IRPartially;
485 exit;
486 end;
487 if cacount<2 then
488 begin
489 result:=IREmpty;
490 exit;
491 end;
492 dec(cacount);
493 d:=VertexSub(lend,lbegin);
494 j:=0;
495 d1:=GetMinAndSwap(j,cacount,ca);
496 while j<=cacount do
497 begin
498 d2:=GetMinAndSwap(j,cacount,ca);
499 d1:=(d1+d2)/2;
500 bit:=0;
501 p:=VertexDmorph(lbegin,d,d1);
502 for i:=0 to 5 do
503 begin
504 if (frustum[i][0] * p.x + frustum[i][1] * p.y + frustum[i][2] * p.z + frustum[i][3])>=0
505 then
506 inc(bit);
507 end;
508 if bit=6 then
509 begin
510 result:=IRPartially;
511 exit;
512 end;
513 d1:=d2;
514 end;
515 end;
CalcTrueInFrustumnull516 function CalcTrueInFrustum (const lbegin,lend:GDBvertex3S; const frustum:ClipArray):TInBoundingVolume;
517 var i,j:GDBInteger;
518 d1,d2:gdbdouble;
519 bytebegin,byteend,bit:integer;
520 ca:TLineClipArray;
521 cacount:integer;
522 d,p:gdbvertex3S;
523 begin
524 fillchar((@ca)^,sizeof(ca),0);
525 result:=IREmpty;
526 bit:=1;
527 bytebegin:=0;
528 byteend:=0;
529 cacount:=0;
530 for i:=0 to 5 do
531 begin
532 d1:=frustum[i][0] * lbegin.x + frustum[i][1] * lbegin.y + frustum[i][2] * lbegin.z + frustum[i][3];
533 d2:=frustum[i][0] * lend.x + frustum[i][1] * lend.y + frustum[i][2] * lend.z + frustum[i][3];
534 if d1<0 then
535 bytebegin:=bytebegin or bit;
536 if d2<0 then
537 byteend:=byteend or bit;
538 if ((bytebegin and bit)and(byteend and bit))>0 then
539 begin
540 result:=IREmpty;
541 exit;
542 end;
543 if((bytebegin and bit)xor(byteend and bit))>0then
544 begin
545 d1:=abs(d1);
546 d2:=abs(d2);
547 ca[cacount]:=d1/(d1+d2);
548 inc(cacount);
549 end;
550 bit:=bit*2;
551 end;
552 if ((bytebegin)=0)and((byteend)=0) then
553 begin
554 result:=IRFully;
555 exit;
556 end;
557
558 if (bytebegin)=0 then
559 begin
560 result:=IRPartially;
561 exit;
562 end;
563 if (byteend)=0 then
564 begin
565 result:=IRPartially;
566 exit;
567 end;
568 if cacount<2 then
569 begin
570 result:=IREmpty;
571 exit;
572 end;
573 dec(cacount);
574 d:=VertexSub(lend,lbegin);
575 j:=0;
576 d1:=GetMinAndSwap(j,cacount,ca);
577 while j<=cacount do
578 begin
579 d2:=GetMinAndSwap(j,cacount,ca);
580 d1:=(d1+d2)/2;
581 bit:=0;
582 p:=VertexDmorph(lbegin,d,d1);
583 for i:=0 to 5 do
584 begin
585 if (frustum[i][0] * p.x + frustum[i][1] * p.y + frustum[i][2] * p.z + frustum[i][3])>=0
586 then
587 inc(bit);
588 end;
589 if bit=6 then
590 begin
591 result:=IRPartially;
592 exit;
593 end;
594 d1:=d2;
595 end;
596 end;
CalcPointTrueInFrustumnull597 function CalcPointTrueInFrustum (const lbegin:GDBvertex; const frustum:ClipArray):TInBoundingVolume;
598 var i{,j}:GDBInteger;
599 d1{,d2}:gdbdouble;
600 //bytebegin,byteend,bit:integer;
601 //ca:TLineClipArray;
602 //cacount:integer;
603 //d,p:gdbvertex;
604 begin
605 for i:=0 to 5 do
606 begin
607 d1:=frustum[i][0] * lbegin.x + frustum[i][1] * lbegin.y + frustum[i][2] * lbegin.z + frustum[i][3];
608 if d1<0 then
609 begin
610 result:=IREmpty;
611 exit;
612 end;
613 end;
614 result:=IRFully;
615 end;
616
CalcAABBInFrustumnull617 function CalcAABBInFrustum (const AABB:TBoundingBox; const frustum:ClipArray):TInBoundingVolume;
618 var i,count:GDBInteger;
619 p1,p2,p3,p4,p5,p6,p7,p8:Gdbvertex;
620 d1,d2,d3,d4,d5,d6,d7,d8:gdbdouble;
621 begin
622 //result:=irfully;
623 //system.exit;
624
625 p1:=AABB.LBN;
626 p2:=CreateVertex(AABB.RTF.x,AABB.LBN.y,AABB.LBN.Z);
627 p3:=CreateVertex(AABB.RTF.x,AABB.RTF.y,AABB.LBN.Z);
628 p4:=CreateVertex(AABB.LBN.x,AABB.RTF.y,AABB.LBN.Z);
629 p5:=CreateVertex(AABB.LBN.x,AABB.LBN.y,AABB.RTF.Z);
630 p6:=CreateVertex(AABB.RTF.x,AABB.LBN.y,AABB.RTF.Z);
631 p7:=AABB.RTF;
632 p8:=CreateVertex(AABB.LBN.x,AABB.RTF.y,AABB.RTF.Z);
633
634 count:=0;
635 for i:=0 to 5 do
636 begin
637 d1:=frustum[i][0] * p1.x + frustum[i][1] * p1.y + frustum[i][2] * p1.z + frustum[i][3];
638 d2:=frustum[i][0] * p2.x + frustum[i][1] * p2.y + frustum[i][2] * p2.z + frustum[i][3];
639 d3:=frustum[i][0] * p3.x + frustum[i][1] * p3.y + frustum[i][2] * p3.z + frustum[i][3];
640 d4:=frustum[i][0] * p4.x + frustum[i][1] * p4.y + frustum[i][2] * p4.z + frustum[i][3];
641 d5:=frustum[i][0] * p5.x + frustum[i][1] * p5.y + frustum[i][2] * p5.z + frustum[i][3];
642 d6:=frustum[i][0] * p6.x + frustum[i][1] * p6.y + frustum[i][2] * p6.z + frustum[i][3];
643 d7:=frustum[i][0] * p7.x + frustum[i][1] * p7.y + frustum[i][2] * p7.z + frustum[i][3];
644 d8:=frustum[i][0] * p8.x + frustum[i][1] * p8.y + frustum[i][2] * p8.z + frustum[i][3];
645
646 if (d1<0)and(d2<0)and(d3<0)and(d4<0)and(d5<0)and(d6<0)and(d7<0)and(d8<0)
647 then
648 begin
649
650 d1:=d2;
651 if d1>d2 then halt(0);
652 result:=irempty;
653 system.exit;
654 end;
655 if d1>=0 then inc(count);
656 if d2>=0 then inc(count);
657 if d3>=0 then inc(count);
658 if d4>=0 then inc(count);
659 if d5>=0 then inc(count);
660 if d6>=0 then inc(count);
661 if d7>=0 then inc(count);
662 if d8>=0 then inc(count);
663 end;
664 if count=48 then
665 begin
666 result:=irfully;
667 exit;
668 end;
669
670 result:=IRPartially;
671 end;
PointOf3PlaneIntersectnull672 function PointOf3PlaneIntersect(const P1,P2,P3:DVector4D):GDBVertex;
673 var
674 N1,N2,N3,N12,N23,N31,a1,a2,a3:GDBVertex;
675 a4:GDBDouble;
676 begin
677 result:=nulvertex;
678 n1:=createvertex(p1[0],p1[1],p1[2]);
679 n2:=createvertex(p2[0],p2[1],p2[2]);
680 n3:=createvertex(p3[0],p3[1],p3[2]);
681 n12:=vectordot(n1,n2);
682 n23:=vectordot(n2,n3);
683 n31:=vectordot(n3,n1);
684
685 a1:=VertexMulOnSc(n23,p1[3]);
686 a2:=VertexMulOnSc(n31,p2[3]);
687 a3:=VertexMulOnSc(n12,p3[3]);
688 a4:=scalardot(n1,n23);
689 if abs(a4)<eps then
690 exit;
691 a4:=1/a4;
692
693 a1:=VertexAdd(a1,a2);
694 a1:=VertexAdd(a1,a3);
695
696 result:=VertexMulOnSc(a1,-a4);
697
698 end;
699 procedure NormalizePlane(var plane:DVector4D);{inline;}
700 var t:GDBDouble;
701 begin
702 t := sqrt( plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2] );
703 plane[0] := plane[0]/t;
704 plane[1] := plane[1]/t;
705 plane[2] := plane[2]/t;
706 plane[3] := plane[3]/t;
707 end;
708
PlaneFrom3Pontnull709 function PlaneFrom3Pont(const P1,P2,P3:GDBVertex):DVector4D;
710 //var
711 // N1,N2,N3,N12,N23,N31,a1,a2,a3:GDBVertex;
712 // a4:GDBDouble;
713 begin
714 result[0]:= P1.y*(P2.z - P3.z) + P2.y* (P3.z - P1.z) + P3.y* (P1.z - P2.z);
715 result[1]:= P1.z*(P2.x - P3.x) + P2.z* (P3.x - P1.x) + P3.z* (P1.x - P2.x);
716 result[2]:= P1.x*(P2.y - P3.y) + P2.x* (P3.y - P1.y) + P3.x* (P1.y - P2.y);
717 result[3]:= -(P1.x*(P2.y*P3.z - P3.y*P2.z) + P2.x*(P3.y*P1.z - P1.y*P3.z) + P3.x*(P1.y*P2.z - P2.y*P1.z));
718
719 end;
PointOfLinePlaneIntersectnull720 function PointOfLinePlaneIntersect(const p1,d:GDBVertex;const plane:DVector4D;out point :GDBVertex):GDBBoolean;
721 var
722 // N1,N2,N3,N12,N23,N31,a1,a2,a3:GDBVertex;
723 td:GDBDouble;
724 begin
725 td:=-plane[0]*d.x-plane[1]*d.y-plane[2]*d.z;
726 if abs(td)<eps then
727 begin
728 result:=false;
729 exit;
730 end;
731 td:=(plane[0]*p1.x+plane[1]*p1.y+plane[2]*p1.z+plane[3])/td;
732 point:=VertexDmorph(p1,d,td);
733 result:=true;
734
735 end;
calcfrustumnull736 function calcfrustum(const clip:PDMatrix4D):cliparray;
737 var t:GDBDouble;
738 begin
739 //* Находим A, B, C, D для ПРАВОЙ плоскости */
740 result[0][0] := clip[0, 3] - clip[0, 0];
741 result[0][1] := clip[1, 3] - clip[1, 0];
742 result[0][2] := clip[2,3] - clip[2,0];
743 result[0][3] := clip[3,3] - clip[3,0];
744 t := sqrt( result[0][0] * result[0][0] + result[0][1] * result[0][1] + result[0][2] * result[0][2] );
745 result[0][0] := result[0][0]/t;
746 result[0][1] := result[0][1]/t;
747 result[0][2] := result[0][2]/t;
748 result[0][3] := result[0][3]/t;
749
750 //* Находим A, B, C, D для ЛЕВОЙ плоскости */
751 result[1][0] := clip[0, 3] + clip[0,0];
752 result[1][1] := clip[1, 3] + clip[1,0];
753 result[1][2] := clip[2,3] + clip[2,0];
754 result[1][3] := clip[3,3] + clip[3,0];
755 t := sqrt( result[1][0] * result[1][0] + result[1][1] * result[1][1] + result[1][2] * result[1][2] );
756 result[1][0] := result[1][0]/t;
757 result[1][1] := result[1][1]/t;
758 result[1][2] := result[1][2]/t;
759 result[1][3] := result[1][3]/t;
760
761 //* Находим A, B, C, D для НИЖНЕЙ плоскости */
762 result[2][0] := clip[ 0,3] + clip[ 0,1];
763 result[2][1] := clip[ 1,3] + clip[ 1,1];
764 result[2][2] := clip[2,3] + clip[ 2,1];
765 result[2][3] := clip[3,3] + clip[3,1];
766 t := sqrt( result[2][0] * result[2][0] + result[2][1] * result[2][1] + result[2][2] * result[2][2] );
767 result[2][0] := result[2][0]/t;
768 result[2][1] := result[2][1]/t;
769 result[2][2] := result[2][2]/t;
770 result[2][3] := result[2][3]/t;
771
772 //* ВЕРХНЯЯ плоскость */
773 result[3][0] := clip[ 0,3] - clip[ 0,1];
774 result[3][1] := clip[ 1,3] - clip[ 1,1];
775 result[3][2] := clip[2,3] - clip[ 2,1];
776 result[3][3] := clip[3,3] - clip[3,1];
777 t := sqrt( result[3][0] * result[3][0] + result[3][1] * result[3][1] + result[3][2] * result[3][2] );
778 result[3][0] := result[3][0]/t;
779 result[3][1] := result[3][1]/t;
780 result[3][2] := result[3][2]/t;
781 result[3][3] := result[3][3]/t;
782
783 //* ПЕРЕДНЯЯ плоскость */
784 result[4][0] := clip[ 0,3] + clip[ 0,2];
785 result[4][1] := clip[ 1,3] + clip[ 1,2];
786 result[4][2] := clip[2,3] + clip[2,2];
787 result[4][3] := clip[3,3] + clip[3,2];
788 t := sqrt( result[4][0] * result[4][0] + result[4][1] * result[4][1] + result[4][2] * result[4][2] );
789 result[4][0] := result[4][0]/t;
790 result[4][1] := result[4][1]/t;
791 result[4][2] := result[4][2]/t;
792 result[4][3] := result[4][3]/t;
793
794 //* ?? плоскость */
795 result[5][0] := clip[ 0,3] - clip[ 0,2];
796 result[5][1] := clip[ 1,3] - clip[ 1,2];
797 result[5][2] := clip[2,3] - clip[2,2];
798 result[5][3] := clip[3,3] - clip[3,2];
799 t := sqrt( result[5][0] * result[5][0] + result[5][1] * result[5][1] + result[5][2] * result[5][2] );
800 result[5][0] := result[5][0]/t;
801 result[5][1] := result[5][1]/t;
802 result[5][2] := result[5][2]/t;
803 result[5][3] := result[5][3]/t;
804 end;
805
806
orthonull807 function ortho;
808 var xmaxminusxmin,ymaxminusymin,zmaxminuszmin,
809 xmaxplusxmin,ymaxplusymin,zmaxpluszmin:GDBDouble;
810 m:DMatrix4D;
811 begin
812 xmaxminusxmin:=xmax-xmin;
813 ymaxminusymin:=ymax-ymin;
814 zmaxminuszmin:=-(zmax-zmin);
815 xmaxplusxmin:=xmax+xmin;
816 ymaxplusymin:=ymax+ymin;
817 zmaxpluszmin:=zmax+zmin;
818 m:=OneMatrix;
819 if (abs(xmaxminusxmin)<eps) or
820 (abs(ymaxminusymin)<eps) or
821 (abs(zmaxminuszmin)<eps) then
822 begin
823 result:=matrix^;
824 exit;
825 end;
826 {Все коэффициенты домножены на xmaxminusxmin, воччтановить оригинал - соответственно всё разделить}
827 m[0,0]:=2{/xmaxminusxmin};
828 m[1,1]:=(2/ymaxminusymin)*xmaxminusxmin;
829 m[2,2]:=(2/zmaxminuszmin)*xmaxminusxmin;
830 m[3,0]:=(-xmaxplusxmin/xmaxminusxmin)*xmaxminusxmin;
831 m[3,1]:=(-ymaxplusymin/ymaxminusymin)*xmaxminusxmin;
832 m[3,2]:=(zmaxpluszmin/zmaxminuszmin)*xmaxminusxmin;
833
834 m[3,3]:=xmaxminusxmin;
835
836 result:=MatrixMultiply(m,matrix^);
837 //glMultMatrixd(@m);
838 end;
839 {function Perspective;
840 var w,h,zmaxminuszmin:GDBDouble;
841 m:DMatrix4D;
842 begin
843 h:=Cotan(fovy*pi/(2*180));
844 w:= h/W_H;
845 zmaxminuszmin:=-(zmax-zmin);
846 m:=EmptyMatrix;
847 m[0,0]:=w;
848 m[1,1]:=h;
849 m[2,2]:=zmax/zmaxminuszmin;
850 m[3,2]:=2*zmin*zmax/zmaxminuszmin;
851 m[2,3]:=-1;
852
853 result:=uzegeometry.MatrixMultiply(m,matrix^);
854 //glMultMatrixd(@m);
855 end;}
856
857
Perspectivenull858 function Perspective;
859 var sine, cotangent, deltaZ, radians:GDBDouble;
860 m:DMatrix4D;
861 begin
862
863 radians:= fovy/2*Pi/180;
864 deltaZ:=zmax - zmin;
865 sine:=sin(radians);
866
867
868 {if ((deltaZ == 0) || (sine == 0) || (aspect == 0))
869 return;
870 }
871
872 cotangent:= COS(radians) / sine;
873
874 m:=OneMatrix;
875
876 m[0,0] := cotangent / w_h;
877 m[1,1] := cotangent;
878 m[2,2] := -(zmax + zmin) / deltaZ;
879 m[2,3] := -1;
880 m[3,2] := -2 * zmin * zmax / deltaZ;
881 m[3,3] := 0;
882
883 result:=MatrixMultiply(m,matrix^);
884 end;
885
886 (*
887
888 gluPerspective(GLdouble fovy, GLdouble aspect, GLdouble zNear, GLdouble zFar)
889 {
890 GLdouble m[4][4];
891 double sine, cotangent, deltaZ;
892 double radians = fovy / 2 * __glPi / 180;
893
894 deltaZ = zFar - zNear;
895 sine = sin(radians);
896 if ((deltaZ == 0) || (sine == 0) || (aspect == 0)) {
897 return;
898 }
899 cotangent = COS(radians) / sine;
900
901 __gluMakeIdentityd(&m[0][0]);
902 m[0][0] = cotangent / aspect;
903 m[1][1] = cotangent;
904 m[2][2] = -(zFar + zNear) / deltaZ;
905 m[2][3] = -1;
906 m[3][2] = -2 * zNear * zFar / deltaZ;
907 m[3][3] = 0;
908 glMultMatrixd(&m[0][0]);
909 }
910
911 *)
912
lookatnull913 function lookat;
914 var m:DMatrix4D;
915 m2:DMatrix4D;
916 begin
917 m:=OneMatrix;
918 m2:=OneMatrix;
919 ex.x:=-ex.x;
920 ex.y:=-ex.y;
921 ex.z:=-ex.z;
922 ez.x:=-ez.x;
923 ez.y:=-ez.y;
924 ez.z:=-ez.z;
925 PGDBVertex(@m[0])^:=ex;
926 PGDBVertex(@m[1])^:=ey;
927 PGDBVertex(@m[2])^:=ez;
928 MatrixTranspose(m);
929 point.x:=point.x;
930 point.y:=point.y;
931 point.z:=point.z;
932 PGDBVertex(@m2[3])^:=point;
933 //m2[3][3]:=1/point.y*point.y;
934
935 m:=MatrixMultiply(M2,M);
936
937 result:=MatrixMultiply(m,matrix^);
938 //glMultMatrixd(@m);
939
940 end;
941 procedure _myGluUnProject(const winx,winy,winz:GDBdouble;const modelMatrix,projMatrix:PDMatrix4D;const viewport:PIMatrix4;out objx,objy,objz:GDBdouble);
942 var
943 _in,_out:GDBVertex4D;
944 finalMatrix:DMatrix4D;
945 begin
946 finalMatrix:=MatrixMultiply(modelMatrix^,projMatrix^);
947 MatrixInvert(finalMatrix);
948
949 _in.x:=winx;
950 _in.y:=winy;
951 _in.z:=winz;
952 _in.w:=1.0;
953
954 _in.x:= (_in.x - viewport[0]) / viewport[2];
955 _in.y:= (_in.y - viewport[1]) / viewport[3];
956
957 //* Map to range -1 to 1 */
958 _in.x:= _in.x * 2 - 1;
959 _in.y:= _in.y * 2 - 1;
960 _in.z:= _in.z * 2 - 1;
961
962 _out:=VectorTransform(_in,finalMatrix);
963
964 _out.x:=_out.x/_out.w;
965 _out.y:=_out.y/_out.w;
966 _out.z:=_out.z/_out.w;
967 objx:= _out.x;
968 objy:= _out.y;
969 objz:= _out.z;
970 end;
971 procedure _myGluProject2(const objcoord:GDBVertex;const modelMatrix,projMatrix:PDMatrix4D;const viewport:PIMatrix4; out wincoord:GDBVertex);
972 begin
973 _myGluProject(objcoord.x,objcoord.y,objcoord.z,modelMatrix,projMatrix,viewport,wincoord.x,wincoord.y,wincoord.z);
974 end;
975 procedure _myGluProject(const objx,objy,objz:GDBdouble;const modelMatrix,projMatrix:PDMatrix4D;const viewport:PIMatrix4; out winx,winy,winz:GDBdouble);
976 var
977 _in,_out:GDBVertex4D;
978 begin
979 _in.x:=objx;
980 _in.y:=objy;
981 _in.z:=objz;
982 _in.w:=1.0;
983 _out:=VectorTransform(_in,modelMatrix^);
984 _in:=VectorTransform(_out,projMatrix^);
985
986 _in.x:=_in.x/_in.w;
987 _in.y:=_in.y/_in.w;
988 _in.z:=_in.z/_in.w;
989
990 //* Map x, y and z to range 0-1 */
991 _in.x:=_in.x * 0.5 + 0.5;
992 _in.y:=_in.y * 0.5 + 0.5;
993 _in.z:=_in.z * 0.5 + 0.5;
994
995 //* Map x,y to viewport */
996 _in.x:=_in.x * viewport[2] + viewport[0];
997 _in.y:=_in.y * viewport[3] + viewport[1];
998
999 winx:=_in.x;
1000 winy:=_in.y;
1001 winz:=_in.z;
1002 //return(GL_TRUE);
1003 end;
vertexeqnull1004 function vertexeq(const v1,v2:gdbvertex):GDBBoolean;
1005 var x,y,z:GDBDouble;
1006 begin
1007 x:=v2.x-v1.x;
1008 y:=v2.y-v1.y;
1009 z:=v2.z-v1.z;
1010 if x*x+y*y+z*z<bigeps then result:=true
1011 else result:=false;
1012 end;
distance2point_2null1013 function distance2point_2(var p1,p2:GDBvertex2DI):GDBInteger;
1014 var x,y:GDBInteger;
1015 begin
1016 x:=p2.x-p1.x;
1017 y:=p2.y-p1.y;
1018 result:=x*x+y*y;
1019 end;
MatrixDetInternalnull1020 function MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3:GDBDouble):GDBDouble;
1021 begin
1022 Result := a1 * (b2 * c3 - b3 * c2) -
1023 b1 * (a2 * c3 - a3 * c2) +
1024 c1 * (a2 * b3 - a3 * b2);
1025 end;
1026 procedure MatrixAdjoint(var M: DMatrix4D);
1027 var a1, a2, a3, a4,
1028 b1, b2, b3, b4,
1029 c1, c2, c3, c4,
1030 d1, d2, d3, d4: GDBDouble;
1031 begin
1032 a1 := M[0, 0]; b1 := M[0, 1];
1033 c1 := M[0, 2]; d1 := M[0, 3];
1034 a2 := M[1, 0]; b2 := M[1, 1];
1035 c2 := M[1, 2]; d2 := M[1, 3];
1036 a3 := M[2, 0]; b3 := M[2, 1];
1037 c3 := M[2, 2]; d3 := M[2, 3];
1038 a4 := M[3, 0]; b4 := M[3, 1];
1039 c4 := M[3, 2]; d4 := M[3, 3];
1040
1041 // row column labeling reversed since we transpose rows & columns
1042 M[XAxisIndex, XAxisIndex] := MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4);
1043 M[YAxisIndex, XAxisIndex] := -MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4);
1044 M[ZAxisIndex, XAxisIndex] := MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4);
1045 M[WAxisIndex, XAxisIndex] := -MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1046
1047 M[XAxisIndex, YAxisIndex] := -MatrixDetInternal(b1, b3, b4, c1, c3, c4, d1, d3, d4);
1048 M[YAxisIndex, YAxisIndex] := MatrixDetInternal(a1, a3, a4, c1, c3, c4, d1, d3, d4);
1049 M[ZAxisIndex, YAxisIndex] := -MatrixDetInternal(a1, a3, a4, b1, b3, b4, d1, d3, d4);
1050 M[WAxisIndex, YAxisIndex] := MatrixDetInternal(a1, a3, a4, b1, b3, b4, c1, c3, c4);
1051
1052 M[XAxisIndex, ZAxisIndex] := MatrixDetInternal(b1, b2, b4, c1, c2, c4, d1, d2, d4);
1053 M[YAxisIndex, ZAxisIndex] := -MatrixDetInternal(a1, a2, a4, c1, c2, c4, d1, d2, d4);
1054 M[ZAxisIndex, ZAxisIndex] := MatrixDetInternal(a1, a2, a4, b1, b2, b4, d1, d2, d4);
1055 M[WAxisIndex, ZAxisIndex] := -MatrixDetInternal(a1, a2, a4, b1, b2, b4, c1, c2, c4);
1056
1057 M[XAxisIndex, WAxisIndex] := -MatrixDetInternal(b1, b2, b3, c1, c2, c3, d1, d2, d3);
1058 M[YAxisIndex, WAxisIndex] := MatrixDetInternal(a1, a2, a3, c1, c2, c3, d1, d2, d3);
1059 M[ZAxisIndex, WAxisIndex] := -MatrixDetInternal(a1, a2, a3, b1, b2, b3, d1, d2, d3);
1060 M[WAxisIndex, WAxisIndex] := MatrixDetInternal(a1, a2, a3, b1, b2, b3, c1, c2, c3);
1061 end;
MatrixDeterminantnull1062 function MatrixDeterminant(M: DMatrix4D): GDBDouble;
1063 var a1, a2, a3, a4,
1064 b1, b2, b3, b4,
1065 c1, c2, c3, c4,
1066 d1, d2, d3, d4 : GDBDouble;
1067
1068 begin
1069 a1 := M[0, 0]; b1 := M[0, 1]; c1 := M[0, 2]; d1 := M[0, 3];
1070 a2 := M[1, 0]; b2 := M[1, 1]; c2 := M[1, 2]; d2 := M[1, 3];
1071 a3 := M[2, 0]; b3 := M[2, 1]; c3 := M[2, 2]; d3 := M[2, 3];
1072 a4 := M[3, 0]; b4 := M[3, 1]; c4 := M[3, 3]; d4 := M[3, 3];
1073
1074 Result := a1 * MatrixDetInternal(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
1075 b1 * MatrixDetInternal(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
1076 c1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
1077 d1 * MatrixDetInternal(a2, a3, a4, b2, b3, b4, c2, c3, c4);
1078 end;
1079 procedure MatrixScale(var M: DMatrix4D; Factor: GDBDouble);
1080 var I, J: GDBInteger;
1081 begin
1082 for I := 0 to 3 do
1083 for J := 0 to 3 do M[I, J] := M[I, J] * Factor;
1084 end;
1085
1086 procedure MatrixInvert(var M: DMatrix4D);
1087 var Det: GDBDouble;
1088 begin
1089 Det := MatrixDeterminant(M);
1090 if Abs(Det) < eps then M := onematrix
1091 else
1092 begin
1093 MatrixAdjoint(M);
1094 MatrixScale(M, 1 / Det);
1095 end;
1096 end;
1097
SQRdist_Point_to_Segmentnull1098 function SQRdist_Point_to_Segment(const p:GDBVertex;const s0,s1:GDBvertex):gdbdouble;
1099 var
1100 v,w,pb:gdbvertex;
1101 c1,c2,b:gdbdouble;
1102 begin
1103 v:=vertexsub(s1,s0);
1104 w:=vertexsub(p,s0);
1105
1106 c1 := scalardot(w,v);
1107 if c1 <= 0 then
1108 begin
1109 result:=SqrVertexlength(p,s0);
1110 exit;
1111 end;
1112
1113 c2:=scalardot(v,v);
1114 if c2 <= c1 then
1115 begin
1116 result:=SqrVertexlength(p,s1);
1117 exit;
1118 end;
1119
1120 b:=c1/c2;
1121 Pb:=vertexadd(s0,VertexMulOnSc(v,b));
1122 result:=SqrVertexlength(p,pb);
1123 end;
NearestPointOnSegmentnull1124 function NearestPointOnSegment(const p:GDBVertex;const s0,s1:GDBvertex):GDBvertex;
1125 var
1126 v,w,pb:gdbvertex;
1127 c1,c2,b:gdbdouble;
1128 begin
1129 v:=vertexsub(s1,s0);
1130 w:=vertexsub(p,s0);
1131
1132 c1 := scalardot(w,v);
1133 if c1 <= 0 then
1134 begin
1135 result:=s0;
1136 exit;
1137 end;
1138
1139 c2:=scalardot(v,v);
1140 if c2 <= c1 then
1141 begin
1142 result:=s1;
1143 exit;
1144 end;
1145
1146 b:=c1/c2;
1147 Pb:=vertexadd(s0,VertexMulOnSc(v,b));
1148 result:=pb;
1149 end;
distance2piecenull1150 function distance2piece(q:GDBvertex;var p1,p2:GDBvertex):{DistAndPoint}double;
1151 var t,w,p2x_p1x,p2y_p1y,qx_p1x,qy_p1y,qy_p2y,qx_p2x: double;
1152 begin
1153 p2x_p1x:=p2.x-p1.x;
1154 p2y_p1y:=p2.y-p1.y;
1155 qx_p1x:=q.x-p1.x;
1156 qx_p2x:=q.x-p2.x;
1157 qy_p1y:=q.y-p1.y;
1158 qy_p2y:=q.y-p2.y;
1159 if((qx_p1x)*(p2x_p1x)+(qy_p1y)*(p2y_p1y))*((qx_p2x)*(p2x_p1x)+(qy_p2y)*(p2y_p1y))>-eps then
1160 begin
1161 t:= sqr(qx_p1x)+sqr(qy_p1y);
1162 w:= sqr(qx_p2x)+sqr(qy_p2y);
1163 if w<t then
1164 begin
1165 t:= w;
1166 //result.point:=p2;
1167 end
1168 else
1169 begin
1170 //result.point:=p1;
1171 end;
1172 end else
1173 begin
1174 t:= sqr((qx_p1x)*(p2y_p1y)-(qy_p1y)*(p2x_p1x))/(sqr(p2x_p1x)+sqr(p2y_p1y));
1175 end;
1176 result{.d}:= sqrt(t);
1177 //result.point:=
1178 end;
distance2raynull1179 function distance2ray(q:GDBvertex;const p1,p2:GDBvertex):DistAndt;
1180 var w,v:gdbvertex;
1181 c1,c2:double;
1182 begin
1183 v:=VertexSub(p2,p1);
1184 w:=VertexSub(q,p1);
1185 c1:=scalardot(w,v);
1186 c2:=scalardot(v,v);
1187 if abs(c2)>eps then
1188 begin
1189 result.t:=c1/c2;
1190 result.d:=Vertexlength(q,VertexDmorph(p1,v,result.t));
1191 end
1192 else
1193 begin
1194 result.t:=0;
1195 result.d:=Vertexlength(q,p1);
1196 end;
1197 end;
distance2piecenull1198 function distance2piece(var q:GDBvertex2DI;var p1,p2:GDBvertex2D): double;
1199 var t,w,p2x_p1x,p2y_p1y,qx_p1x,qy_p1y,qy_p2y,qx_p2x: double;
1200 begin
1201 p2x_p1x:=p2.x-p1.x;
1202 p2y_p1y:=p2.y-p1.y;
1203 qx_p1x:=q.x-p1.x;
1204 qx_p2x:=q.x-p2.x;
1205 qy_p1y:=q.y-p1.y;
1206 qy_p2y:=q.y-p2.y;
1207 if((qx_p1x)*(p2x_p1x)+(qy_p1y)*(p2y_p1y))*((qx_p2x)*(p2x_p1x)+(qy_p2y)*(p2y_p1y))>-eps then
1208 begin
1209 t:= sqr(qx_p1x)+sqr(qy_p1y);
1210 w:= sqr(qx_p2x)+sqr(qy_p2y);
1211 if w<t then t:= w;
1212 end else
1213 t:= sqr((qx_p1x)*(p2y_p1y)-(qy_p1y)*(p2x_p1x))/(sqr(p2x_p1x)+sqr(p2y_p1y));
1214 result:= sqrt(t);
1215 end;
distance2piece_2null1216 function distance2piece_2(var q:GDBvertex2DI; p1,p2:GDBvertex2D): double;
1217 var t,w,p2x_p1x,p2y_p1y,qx_p1x,qy_p1y,qy_p2y,qx_p2x: double;
1218 begin
1219 p2x_p1x:=p2.x-p1.x;
1220 p2y_p1y:=p2.y-p1.y;
1221 qx_p1x:=q.x-p1.x;
1222 qx_p2x:=q.x-p2.x;
1223 qy_p1y:=q.y-p1.y;
1224 qy_p2y:=q.y-p2.y;
1225 if((qx_p1x)*(p2x_p1x)+(qy_p1y)*(p2y_p1y))*((qx_p2x)*(p2x_p1x)+(qy_p2y)*(p2y_p1y))>-eps then
1226 begin
1227 t:= sqr(qx_p1x)+sqr(qy_p1y);
1228 w:= sqr(qx_p2x)+sqr(qy_p2y);
1229 if w<t then t:= w;
1230 end else
1231 t:= sqr((qx_p1x)*(p2y_p1y)-(qy_p1y)*(p2x_p1x))/(sqr(p2x_p1x)+sqr(p2y_p1y));
1232 result:= t;
1233 end;
distance2piece_2null1234 function distance2piece_2(var q:GDBvertex2DI; p1,p2:GDBvertex2DI): double;
1235 var t,w,p2x_p1x,p2y_p1y,qx_p1x,qy_p1y,qy_p2y,qx_p2x: double;
1236 begin
1237 p2x_p1x:=p2.x-p1.x;
1238 p2y_p1y:=p2.y-p1.y;
1239 qx_p1x:=q.x-p1.x;
1240 qx_p2x:=q.x-p2.x;
1241 qy_p1y:=q.y-p1.y;
1242 qy_p2y:=q.y-p2.y;
1243 if((qx_p1x)*(p2x_p1x)+(qy_p1y)*(p2y_p1y))*((qx_p2x)*(p2x_p1x)+(qy_p2y)*(p2y_p1y))>-eps then
1244 begin
1245 t:= sqr(qx_p1x)+sqr(qy_p1y);
1246 w:= sqr(qx_p2x)+sqr(qy_p2y);
1247 if w<t then t:= w;
1248 end else
1249 t:= sqr((qx_p1x)*(p2y_p1y)-(qy_p1y)*(p2x_p1x))/(sqr(p2x_p1x)+sqr(p2y_p1y));
1250 result:= t;
1251 end;
distance2piece_2dmynull1252 function distance2piece_2dmy(var q:GDBvertex2D; p1,p2:GDBvertex2D): double;
1253 var t,w,p2x_p1x,p2y_p1y,qx_p1x,qy_p1y,qy_p2y,qx_p2x: double;
1254 begin
1255 p2x_p1x:=p2.x-p1.x;
1256 p2y_p1y:=p2.y-p1.y;
1257 qx_p1x:=q.x-p1.x;
1258 qx_p2x:=q.x-p2.x;
1259 qy_p1y:=q.y-p1.y;
1260 qy_p2y:=q.y-p2.y;
1261 if((qx_p1x)*(p2x_p1x)+(qy_p1y)*(p2y_p1y))*((qx_p2x)*(p2x_p1x)+(qy_p2y)*(p2y_p1y))>-eps then
1262 begin
1263 t:= sqr(qx_p1x)+sqr(qy_p1y);
1264 w:= sqr(qx_p2x)+sqr(qy_p2y);
1265 if w<t then t:= w;
1266 end else
1267 t:= sqr((qx_p1x)*(p2y_p1y)-(qy_p1y)*(p2x_p1x))/(sqr(p2x_p1x)+sqr(p2y_p1y));
1268 result:= t;
1269 end;
1270
distance2piece_2_xynull1271 function distance2piece_2_xy(var q:GDBvertex2DI;const p1,p2:GDBvertex2D):GDBvertex2DI;
1272 var t,w,p2x_p1x,p2y_p1y,qx_p1x,qy_p1y,qy_p2y,qx_p2x,s1,s2: double;
1273 begin
1274 p2x_p1x:=p2.x-p1.x;
1275 p2y_p1y:=p2.y-p1.y;
1276 qx_p1x:=q.x-p1.x;
1277 qx_p2x:=q.x-p2.x;
1278 qy_p1y:=q.y-p1.y;
1279 qy_p2y:=q.y-p2.y;
1280 s1:=(qx_p1x)*(p2x_p1x)+(qy_p1y)*(p2y_p1y);
1281 s2:=(qx_p2x)*(p2x_p1x)+(qy_p2y)*(p2y_p1y);
1282 if(s1)*(s2)>-eps then
1283 begin
1284 t:= sqr(qx_p1x)+sqr(qy_p1y);
1285 w:= sqr(qx_p2x)+sqr(qy_p2y);
1286 if w<t then
1287 begin
1288 //t:= w;
1289 result.x:=round(qx_p2x);
1290 result.y:=round(qy_p2y);
1291
1292 end
1293 else
1294 begin
1295 result.x:=round(qx_p1x);
1296 result.y:=round(qy_p1y);
1297 end;
1298 end else
1299 begin
1300 s1:=abs(s1/(abs(s1)+abs(s2)));
1301
1302 result.x:=round(qx_p1x-p2x_p1x*s1);
1303 result.y:=round(qy_p1y-p2y_p1y*s1);
1304
1305 {t:= sqr((qx_p1x)*(p2y_p1y)-(qy_p1y)*(p2x_p1x))/(sqr(p2x_p1x)+sqr(p2y_p1y));
1306 result:= t;}
1307 end;
1308 end;
1309
CreateTranslationMatrixnull1310 function CreateTranslationMatrix(const V:GDBvertex): DMatrix4D;
1311 begin
1312 Result := onematrix;
1313 Result[3, 0] := V.x;
1314 Result[3, 1] := V.y;
1315 Result[3, 2] := V.z;
1316 Result[3, 3] := 1;
1317 end;
CreateReflectionMatrixnull1318 function CreateReflectionMatrix(plane:DVector4D): DMatrix4D;
1319 begin
1320 result[0,0] :=-2 * plane[0] * plane[0] + 1;
1321 result[1,0] :=-2 * plane[0] * plane[1];
1322 result[2,0] :=-2 * plane[0] * plane[2];
1323 result[3,0] :=-2 * plane[0] * plane[3];
1324
1325 result[0,1] :=-2 * plane[1] * plane[0];
1326 result[1,1] :=-2 * plane[1] * plane[1] + 1;
1327 result[2,1] :=-2 * plane[1] * plane[2];
1328 result[3,1] :=-2 * plane[1] * plane[3];
1329
1330 result[0,2] :=-2 * plane[2] * plane[0];
1331 result[1,2] :=-2 * plane[2] * plane[1];
1332 result[2,2] :=-2 * plane[2] * plane[2] + 1;
1333 result[3,2] :=-2 * plane[2] * plane[3];
1334
1335 result[0,3]:=0;
1336 result[1,3]:=0;
1337 result[2,3]:=0;
1338 result[3,3]:=1;
1339 end;
1340
CreateScaleMatrixnull1341 function CreateScaleMatrix(const V:GDBvertex): DMatrix4D;
1342 begin
1343 Result := onematrix;
1344 Result[0, 0] := V.x;
1345 Result[1, 1] := V.y;
1346 Result[2, 2] := V.z;
1347 Result[3, 3] := 1;
1348 end;
1349
CreateRotationMatrixXnull1350 function CreateRotationMatrixX(const Sine, Cosine: GDBDouble): DMatrix4D;
1351 begin
1352 Result := EmptyMatrix;
1353 Result[0, 0] := 1;
1354 Result[1, 1] := Cosine;
1355 Result[1, 2] := Sine;
1356 Result[2, 1] := -Sine;
1357 Result[2, 2] := Cosine;
1358 Result[3, 3] := 1;
1359 end;
CreateRotationMatrixYnull1360 function CreateRotationMatrixY(const Sine, Cosine: GDBDouble): DMatrix4D;
1361 begin
1362 Result := EmptyMatrix;
1363 Result[0, 0] := Cosine;
1364 Result[0, 2] := -Sine;
1365 Result[1, 1] := 1;
1366 Result[2, 0] := Sine;
1367 Result[2, 2] := Cosine;
1368 Result[3, 3] := 1;
1369 end;
CreateRotatedXVectornull1370 function CreateRotatedXVector(const angle: GDBDouble):GDBVertex;
1371 begin
1372 Result.x:=cos(angle);
1373 Result.y:=sin(angle);
1374 Result.z:=0;
1375 end;
CreateRotatedYVectornull1376 function CreateRotatedYVector(const angle: GDBDouble):GDBVertex;
1377 begin
1378 Result:=CreateRotatedXVector(angle+pi/2);
1379 end;
CreateRotationMatrixZnull1380 function CreateRotationMatrixZ(const Sine, Cosine: GDBDouble): DMatrix4D;
1381 begin
1382 Result := Onematrix;
1383 Result[0, 0] := Cosine;
1384 Result[1, 1] := Cosine;
1385 Result[1, 0] := -Sine;
1386 Result[0, 1] := Sine;
1387
1388 end;
CreateAffineRotationMatrixnull1389 function CreateAffineRotationMatrix(const anAxis: GDBvertex; angle: double):DMatrix4D;
1390 var
1391 axis : GDBvertex;
1392 cosine, sine, one_minus_cosine :double;
1393 begin
1394 SINE:=sin(angle);
1395 cosine:=cos(angle);
1396 one_minus_cosine:=1 - cosine;
1397 axis:=NormalizeVertex(anAxis);
1398 result:=onematrix;
1399 Result[XAxisIndex, XAxisIndex]:=(one_minus_cosine * Sqr(Axis.x)) + Cosine;
1400 Result[XAxisIndex, YAxisIndex]:=(one_minus_cosine * Axis.x * Axis.y) - (Axis.z * Sine);
1401 Result[XAxisIndex, ZAxisIndex]:=(one_minus_cosine * Axis.z * Axis.x) + (Axis.y * Sine);
1402
1403 Result[YAxisIndex, XAxisIndex]:=(one_minus_cosine * Axis.x * Axis.y) + (Axis.z * Sine);
1404 Result[YAxisIndex, YAxisIndex]:=(one_minus_cosine * Sqr(Axis.y)) + Cosine;
1405 Result[YAxisIndex, ZAxisIndex]:=(one_minus_cosine * Axis.y * Axis.z) - (Axis.x * Sine);
1406
1407 Result[ZAxisIndex, XAxisIndex]:=(one_minus_cosine * Axis.z * Axis.x) - (Axis.y * Sine);
1408 Result[ZAxisIndex, YAxisIndex]:=(one_minus_cosine * Axis.y * Axis.z) + (Axis.x * Sine);
1409 Result[ZAxisIndex, ZAxisIndex]:=(one_minus_cosine * Sqr(Axis.z)) + Cosine;
1410 end;
1411
MatrixMultiplynull1412 function MatrixMultiply(const M1, M2: DMatrix4D): DMatrix4D;
1413
1414 var I, J: GDBInteger;
1415 TM: DMatrix4D;
1416
1417 begin
1418 for I := 0 to 3 do
1419 for J := 0 to 3 do
1420 TM[I, J] := M1[I,0] * M2[0,J] +
1421 M1[I,1] * M2[1,J] +
1422 M1[I,2] * M2[2,J] +
1423 M1[I,3] * M2[3,J];
1424 Result := TM;
1425 end;
MatrixMultiplynull1426 function MatrixMultiply(const M1: DMatrix4D; M2: DMatrix4F):DMatrix4D;
1427
1428 var I, J: GDBInteger;
1429 TM: DMatrix4D;
1430
1431 begin
1432 for I := 0 to 3 do
1433 for J := 0 to 3 do
1434 TM[I, J] := M1[I,0] * M2[0,J] +
1435 M1[I,1] * M2[1,J] +
1436 M1[I,2] * M2[2,J] +
1437 M1[I,3] * M2[3,J];
1438 Result := TM;
1439 end;
MatrixMultiplyFnull1440 function MatrixMultiplyF(const M1, M2: DMatrix4D):DMatrix4F;
1441
1442 var I, J: GDBInteger;
1443 TM: DMatrix4F;
1444
1445 begin
1446 for I := 0 to 3 do
1447 for J := 0 to 3 do
1448 TM[I, J] := M1[I,0] * M2[0,J] +
1449 M1[I,1] * M2[1,J] +
1450 M1[I,2] * M2[2,J] +
1451 M1[I,3] * M2[3,J];
1452 Result := TM;
1453 end;
1454 procedure MatrixTranspose(var M: DMatrix4D);
1455 var I, J: GDBInteger;
1456 TM: DMatrix4D;
1457 begin
1458 for I := 0 to 3 do
1459 for J := 0 to 3 do TM[J, I] := M[I, J];
1460 M := TM;
1461 end;
1462 procedure MatrixTranspose(var M: DMatrix4F);
1463 var I, J: GDBInteger;
1464 TM: DMatrix4F;
1465 begin
1466 for I := 0 to 3 do
1467 for J := 0 to 3 do TM[J, I] := M[I, J];
1468 M := TM;
1469 end;
1470 procedure MatrixNormalize(var M: DMatrix4D);
1471 var I, J: GDBInteger;
1472 begin
1473 for I := 0 to 3 do
1474 for J := 0 to 3 do
1475 M[I,J]:=M[I,J]/M[3,3];
1476 end;
1477
VectorTransformnull1478 function VectorTransform(const V:GDBVertex4D;const M:DMatrix4D):GDBVertex4D;
1479 var TV: GDBVertex4D;
1480 begin
1481 TV.X := V.X * M[0, 0] + V.y * M[1, 0] + V.z * M[2, 0] + V.w * M[3, 0];
1482 TV.Y := V.X * M[0, 1] + V.y * M[1, 1] + V.z * M[2, 1] + V.w * M[3, 1];
1483 TV.z := V.x * M[0, 2] + V.y * M[1, 2] + V.z * M[2, 2] + V.w * M[3, 2];
1484 TV.W := V.x * M[0, 3] + V.y * M[1, 3] + V.z * M[2, 3] + V.w * M[3, 3];
1485
1486 Result := TV
1487 end;
VectorTransformnull1488 function VectorTransform(const V:GDBVertex4D;const M:DMatrix4F):GDBVertex4D;
1489 var TV: GDBVertex4D;
1490 begin
1491 TV.X := V.X * M[0, 0] + V.y * M[1, 0] + V.z * M[2, 0] + V.w * M[3, 0];
1492 TV.Y := V.X * M[0, 1] + V.y * M[1, 1] + V.z * M[2, 1] + V.w * M[3, 1];
1493 TV.z := V.x * M[0, 2] + V.y * M[1, 2] + V.z * M[2, 2] + V.w * M[3, 2];
1494 TV.W := V.x * M[0, 3] + V.y * M[1, 3] + V.z * M[2, 3] + V.w * M[3, 3];
1495
1496 Result := TV
1497 end;
VectorTransformnull1498 function VectorTransform(const V:GDBVertex4F;const M:DMatrix4F):GDBVertex4F;
1499 var TV: GDBVertex4F;
1500 begin
1501 TV.X := V.X * M[0, 0] + V.y * M[1, 0] + V.z * M[2, 0] + V.w * M[3, 0];
1502 TV.Y := V.X * M[0, 1] + V.y * M[1, 1] + V.z * M[2, 1] + V.w * M[3, 1];
1503 TV.z := V.x * M[0, 2] + V.y * M[1, 2] + V.z * M[2, 2] + V.w * M[3, 2];
1504 TV.W := V.x * M[0, 3] + V.y * M[1, 3] + V.z * M[2, 3] + V.w * M[3, 3];
1505
1506 Result := TV
1507 end;
1508 procedure normalize4F(var tv:GDBVertex4F);
1509 begin
1510 if abs(tv.w)>eps then
1511 if abs(abs(tv.w)-1)>eps then
1512 begin
1513 tv.x:=tv.x/tv.w;
1514 tv.y:=tv.y/tv.w;
1515 tv.z:=tv.z/tv.w;
1516 end;
1517 end;
1518 procedure normalize4d(var tv:GDBVertex4d);
1519 begin
1520 if abs(tv.w)>eps then
1521 if abs(abs(tv.w)-1)>eps then
1522 begin
1523 tv.x:=tv.x/tv.w;
1524 tv.y:=tv.y/tv.w;
1525 tv.z:=tv.z/tv.w;
1526 end;
1527 end;
VectorTransform3Dnull1528 function VectorTransform3D(const V:GDBVertex;const M:DMatrix4D):GDBVertex;
1529 var TV: GDBVertex4D;
1530 begin
1531 pgdbvertex(@tv)^:=v;
1532 tv.w:=1;
1533 tv:=VectorTransform(tv,m);
1534
1535 normalize4d(tv);
1536
1537 Result := pgdbvertex(@tv)^
1538 end;
VectorTransform3Dnull1539 function VectorTransform3D(const V:GDBVertex;const M:DMatrix4F):GDBVertex;
1540 var TV: GDBVertex4D;
1541 begin
1542 pgdbvertex(@tv)^:=v;
1543 tv.w:=1;
1544 tv:=VectorTransform(tv,m);
1545
1546 normalize4d(tv);
1547
1548 Result := pgdbvertex(@tv)^
1549 end;
VectorTransform3Dnull1550 function VectorTransform3D(const V:GDBVertex3S;const M:DMatrix4D):GDBVertex3S;
1551 var tv: GDBVertex4D;
1552 begin
1553 tv.x:=v.x;
1554 tv.y:=v.y;
1555 tv.z:=v.z;
1556 tv.w:=1;
1557 tv:=VectorTransform(tv,m);
1558 normalize4d(tv);
1559 result.x:=tv.x;
1560 result.y:=tv.y;
1561 result.z:=tv.z;
1562 end;
VectorTransform3Dnull1563 function VectorTransform3D(const V:GDBVertex3S;const M:DMatrix4F):GDBVertex3S;
1564 var tv: GDBVertex4F;
1565 begin
1566 tv.x:=v.x;
1567 tv.y:=v.y;
1568 tv.z:=v.z;
1569 tv.w:=1;
1570 tv:=VectorTransform(tv,m);
1571 normalize4f(tv);
1572 result.x:=tv.x;
1573 result.y:=tv.y;
1574 result.z:=tv.z;
1575 end;
FrustumTransformnull1576 function FrustumTransform(const frustum:ClipArray;const M:DMatrix4D; MatrixAlreadyTransposed:Boolean=false):ClipArray;
1577 var
1578 m1:DMatrix4D;
1579 begin
1580 if MatrixAlreadyTransposed
1581 then
1582 begin
1583 PGDBVertex4D(@result[0])^:=VectorTransform(PGDBVertex4D(@frustum[0])^,M);
1584 PGDBVertex4D(@result[1])^:=VectorTransform(PGDBVertex4D(@frustum[1])^,M);
1585 PGDBVertex4D(@result[2])^:=VectorTransform(PGDBVertex4D(@frustum[2])^,M);
1586 PGDBVertex4D(@result[3])^:=VectorTransform(PGDBVertex4D(@frustum[3])^,M);
1587 PGDBVertex4D(@result[4])^:=VectorTransform(PGDBVertex4D(@frustum[4])^,M);
1588 PGDBVertex4D(@result[5])^:=VectorTransform(PGDBVertex4D(@frustum[5])^,M);
1589 end
1590 else
1591 begin
1592 m1:=M;
1593 MatrixTranspose(m1);
1594 PGDBVertex4D(@result[0])^:=VectorTransform(PGDBVertex4D(@frustum[0])^,m1);
1595 PGDBVertex4D(@result[1])^:=VectorTransform(PGDBVertex4D(@frustum[1])^,m1);
1596 PGDBVertex4D(@result[2])^:=VectorTransform(PGDBVertex4D(@frustum[2])^,m1);
1597 PGDBVertex4D(@result[3])^:=VectorTransform(PGDBVertex4D(@frustum[3])^,m1);
1598 PGDBVertex4D(@result[4])^:=VectorTransform(PGDBVertex4D(@frustum[4])^,m1);
1599 PGDBVertex4D(@result[5])^:=VectorTransform(PGDBVertex4D(@frustum[5])^,m1);
1600 end;
1601 end;
FrustumTransformnull1602 function FrustumTransform(const frustum:ClipArray;const M:DMatrix4F; MatrixAlreadyTransposed:Boolean=false):ClipArray;
1603 var
1604 m1:DMatrix4F;
1605 begin
1606 if MatrixAlreadyTransposed
1607 then
1608 begin
1609 PGDBVertex4D(@result[0])^:=VectorTransform(PGDBVertex4D(@frustum[0])^,M);
1610 PGDBVertex4D(@result[1])^:=VectorTransform(PGDBVertex4D(@frustum[1])^,M);
1611 PGDBVertex4D(@result[2])^:=VectorTransform(PGDBVertex4D(@frustum[2])^,M);
1612 PGDBVertex4D(@result[3])^:=VectorTransform(PGDBVertex4D(@frustum[3])^,M);
1613 PGDBVertex4D(@result[4])^:=VectorTransform(PGDBVertex4D(@frustum[4])^,M);
1614 PGDBVertex4D(@result[5])^:=VectorTransform(PGDBVertex4D(@frustum[5])^,M);
1615 end
1616 else
1617 begin
1618 m1:=M;
1619 MatrixTranspose(m1);
1620 PGDBVertex4D(@result[0])^:=VectorTransform(PGDBVertex4D(@frustum[0])^,m1);
1621 PGDBVertex4D(@result[1])^:=VectorTransform(PGDBVertex4D(@frustum[1])^,m1);
1622 PGDBVertex4D(@result[2])^:=VectorTransform(PGDBVertex4D(@frustum[2])^,m1);
1623 PGDBVertex4D(@result[3])^:=VectorTransform(PGDBVertex4D(@frustum[3])^,m1);
1624 PGDBVertex4D(@result[4])^:=VectorTransform(PGDBVertex4D(@frustum[4])^,m1);
1625 PGDBVertex4D(@result[5])^:=VectorTransform(PGDBVertex4D(@frustum[5])^,m1);
1626 end;
1627 end;
Vertexlengthnull1628 function Vertexlength(const Vector1, Vector2: GDBVertex): GDBDouble;
1629 begin
1630 result := sqrt(sqr(vector1.x - vector2.x) + sqr(vector1.y - vector2.y) + sqr(vector1.z - vector2.z));
1631 end;
SqrVertexlengthnull1632 function SqrVertexlength(const Vector1, Vector2: GDBVertex): GDBDouble;
1633 begin
1634 result := (sqr(vector1.x - vector2.x) + sqr(vector1.y - vector2.y) + sqr(vector1.z - vector2.z));
1635 end;
SqrVertexlengthnull1636 function SqrVertexlength(const Vector1, Vector2: GDBVertex2d): GDBDouble;
1637 begin
1638 result := (sqr(vector1.x - vector2.x) + sqr(vector1.y - vector2.y));
1639 end;
1640
oneVertexlengthnull1641 function oneVertexlength(const Vector1: GDBVertex): GDBDouble;
1642 begin
1643 result := sqrt(sqr(vector1.x) + sqr(vector1.y) + sqr(vector1.z));
1644 end;
1645
SqrOneVertexlengthnull1646 function SqrOneVertexlength(const Vector1: GDBVertex): GDBDouble;
1647 begin
1648 result := (sqr(vector1.x) + sqr(vector1.y) + sqr(vector1.z));
1649 end;
1650
vertexlen2dfnull1651 function vertexlen2df(const x1, y1, x2, y2: GDBFloat): GDBFloat;
1652 begin
1653 result := sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
1654 end;
1655
vertexlen2idnull1656 function vertexlen2id(const x1, y1, x2, y2: GDBInteger): GDBDouble;
1657 var a,b:GDBDouble;
1658 begin
1659 a:=x1 - x2;
1660 b:=y1 - y2;
1661 result := sqrt(a*a + b*b);
1662 end;
1663
Vertexanglenull1664 function Vertexangle(const Vector1, Vector2: GDBVertex2d): GDBDouble;
1665 var
1666 dx, dy, temp: GDBDouble;
1667 begin
1668 dx := vector2.x - vector1.x;
1669 dy := vector2.y - vector1.y;
1670 if dx <> 0 then
1671 temp := arctan(abs(dy / dx))
1672 else
1673 temp := {arcotan(abs(dx / dy))}pi/2;
1674 if (dx >= 0) and (dy >= 0) then
1675 result := temp
1676 else
1677 if (dx < 0) and (dy >= 0) then
1678 result := pi - temp
1679 else
1680 if (dx <= 0) and (dy <= 0) then
1681 result := pi + temp
1682 else
1683 if (dx > 0) and (dy < 0) then
1684 result := 2 * pi - temp
1685 end;
TwoVectorAnglenull1686 function TwoVectorAngle(const Vector1, Vector2: GDBVertex): GDBDouble;inline;
1687 var
1688 dx, dy, temp: GDBDouble;
1689 begin
1690 temp:=scalardot(Vector1, Vector2);
1691 Result:=ArcCos(temp);
1692 end;
Vertexmorphnull1693 function Vertexmorph(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;
1694 var
1695 temp: GDBVertex;
1696 begin
1697 temp.x := vector1.x + (vector2.x - vector1.x) * a;
1698 temp.y := vector1.y + (vector2.y - vector1.y) * a;
1699 temp.z := vector1.z + (vector2.z - vector1.z) * a;
1700 result := temp;
1701 end;
Vertexmorphnull1702 function Vertexmorph(const Vector1, Vector2: GDBVertex2D; a: GDBDouble): GDBVertex2d;
1703 begin
1704 result.x := vector1.x + (vector2.x - vector1.x) * a;
1705 result.y := vector1.y + (vector2.y - vector1.y) * a;
1706 end;
1707
VertexDmorphnull1708 function VertexDmorph(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;
1709 var
1710 temp: GDBVertex;
1711 begin
1712 temp.x := vector1.x + (vector2.x) * a;
1713 temp.y := vector1.y + (vector2.y) * a;
1714 temp.z := vector1.z + (vector2.z) * a;
1715 result := temp;
1716 end;
VertexDmorphnull1717 function VertexDmorph(const Vector1, Vector2: GDBVertex3S; a: GDBDouble): GDBVertex3S;
1718 var
1719 temp: GDBVertex3S;
1720 begin
1721 temp.x := vector1.x + (vector2.x) * a;
1722 temp.y := vector1.y + (vector2.y) * a;
1723 temp.z := vector1.z + (vector2.z) * a;
1724 result := temp;
1725 end;
1726
Vertexdmorphabsnull1727 function Vertexdmorphabs(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;
1728 var
1729 temp: GDBVertex;
1730 l: GDBDouble;
1731 begin
1732 l := oneVertexlength(Vector2);
1733 if a > 0 then a := a / l
1734 else a := 1 + a / l;
1735 temp.x := vector1.x + (vector2.x) * a;
1736 temp.y := vector1.y + (vector2.y) * a;
1737 temp.z := vector1.z + (vector2.z) * a;
1738 result := temp;
1739 end;
1740
Vertexmorphabsnull1741 function Vertexmorphabs(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;
1742 var
1743 temp: GDBVertex;
1744 l: GDBDouble;
1745 begin
1746 l := Vertexlength(Vector1, Vector2);
1747 if a > 0 then a := 1+a / l
1748 else a := 1 + a / l;
1749 temp.x := vector1.x + (vector2.x - vector1.x) * a;
1750 temp.y := vector1.y + (vector2.y - vector1.y) * a;
1751 temp.z := vector1.z + (vector2.z - vector1.z) * a;
1752 result := temp;
1753 end;
Vertexmorphabs2null1754 function Vertexmorphabs2(const Vector1, Vector2: GDBVertex; a: GDBDouble): GDBVertex;
1755 var
1756 temp: GDBVertex;
1757 l: GDBDouble;
1758 begin
1759 l := Vertexlength(Vector1, Vector2);
1760 if a > 0 then a := a / l
1761 else a := 1 + a / l;
1762 temp.x := vector1.x + (vector2.x - vector1.x) * a;
1763 temp.y := vector1.y + (vector2.y - vector1.y) * a;
1764 temp.z := vector1.z + (vector2.z - vector1.z) * a;
1765 result := temp;
1766 end;
1767
NormalizeVertexnull1768 function NormalizeVertex(const Vector1: GDBVertex): GDBVertex;
1769 var len:GDBDouble;
1770 begin
1771 len:=oneVertexlength(Vector1);
1772 if abs(len)>eps then
1773 begin
1774 Result.X := Vector1.x / len;
1775 Result.Y := Vector1.y / len;
1776 Result.Z := Vector1.z / len;
1777 end
1778 else
1779 begin
1780 DebugLn('{EH}'+rsDivByZero);
1781 len:=len+2;
1782 end;
1783 end;
VertexMulOnScnull1784 function VertexMulOnSc(const Vector1:GDBVertex;sc:GDBDouble): GDBVertex;
1785 begin
1786 Result.X := Vector1.x*sc;
1787 Result.Y := Vector1.y*sc;
1788 Result.Z := Vector1.z*sc;
1789 end;
VertexAddnull1790 function VertexAdd(const Vector1, Vector2: GDBVertex): GDBVertex;
1791 begin
1792 Result.X := Vector1.x + Vector2.x;
1793 Result.Y := Vector1.y + Vector2.y;
1794 Result.Z := Vector1.z + Vector2.z;
1795 end;
VertexAddnull1796 function VertexAdd(const Vector1, Vector2: GDBVertex3S): GDBVertex3s;
1797 begin
1798 Result.X := Vector1.x + Vector2.x;
1799 Result.Y := Vector1.y + Vector2.y;
1800 Result.Z := Vector1.z + Vector2.z;
1801 end;
Vertex2DAddnull1802 function Vertex2DAdd(const Vector1, Vector2: GDBVertex2D): GDBVertex2D;
1803 begin
1804 Result.X := Vector1.x + Vector2.x;
1805 Result.Y := Vector1.y + Vector2.y;
1806 end;
1807
VertexSubnull1808 function VertexSub(const Vector1, Vector2: GDBVertex): GDBVertex;
1809 begin
1810 Result.X := Vector1.x - Vector2.x;
1811 Result.Y := Vector1.y - Vector2.y;
1812 Result.Z := Vector1.z - Vector2.z;
1813 end;
VertexSubnull1814 function VertexSub(const Vector1, Vector2: GDBvertex3S): GDBVertex3S;
1815 begin
1816 Result.X := Vector1.x - Vector2.x;
1817 Result.Y := Vector1.y - Vector2.y;
1818 Result.Z := Vector1.z - Vector2.z;
1819 end;
MinusVertexnull1820 function MinusVertex(const Vector1: GDBVertex): GDBVertex;
1821 begin
1822 Result.X := -Vector1.x;
1823 Result.Y := -Vector1.y;
1824 Result.Z := -Vector1.z;
1825 end;
CrossVertexnull1826 function CrossVertex;
1827 begin
1828 Result.X := (Vector1.Y * Vector2.Z) - (Vector1.Z * Vector2.Y);
1829 Result.Y := (Vector1.Z * Vector2.X) - (Vector1.X * Vector2.Z);
1830 Result.Z := (Vector1.X * Vector2.Y) - (Vector1.Y * Vector2.X);
1831 end;
1832
intercept2dnull1833 function intercept2d(const x1, y1, x2, y2, x3, y3, x4, y4: GDBDouble): GDBBoolean;
1834 var
1835 z1, z2: GDBDouble;
1836 begin
1837 z1 := (x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1);
1838 z2 := (x4 - x1) * (y2 - y1) - (y4 - y1) * (x2 - x1);
1839 if z1 * z2 > 0 then
1840 result := false
1841 else
1842 result := true;
1843 end;
1844
pointinquad2dnull1845 function pointinquad2d(const x1, y1, x2, y2, xp, yp: GDBFloat): GDBBoolean;
1846 begin
1847 if (x1 <= xp) and (x2 >= xp) and (y1 <= yp) and (y2 >= yp) then result := true
1848 else result := false;
1849 end;
intercept2dmynull1850 function intercept2dmy(const l1begin,l1end,l2begin,l2end:gdbvertex2d):intercept2dprop;
1851 var
1852 {z,} {t,} t1, t2, d, d1, d2: GDBDouble;
1853 begin
1854 //t := 0;
1855 //t1 := 0;
1856 //t2 := 0;
1857 result.isintercept := false;
1858 D := (l1end.y - l1begin.y) * (l2begin.x - l2end.x) - (l2begin.y - l2end.y) * (l1end.x - l1begin.x);
1859 D1 := (l1end.y - l1begin.y) * (l2begin.x - l1begin.x) - (l2begin.y - l1begin.y) * (l1end.x - l1begin.x);
1860 D2 := (l2begin.y - l1begin.y) * (l2begin.x - l2end.x) - (l2begin.y - l2end.y) * (l2begin.x - l1begin.x);
1861 if (D <> 0) then
1862 begin
1863 t1 := D1 / D;
1864 t2 := D2 / D;
1865 //if ((t1 <= 1) and (t1 >= 0) and (t2 >= 0) and (t2 <= 1)) then
1866 begin
1867 result.interceptcoord.x := l1begin.x + (l1end.x - l1begin.x) * t2;
1868 result.interceptcoord.y := l1begin.y + (l1end.y - l1begin.y) * t2;
1869 //if abs(result.interceptcoord.z-z)<eps then
1870 begin
1871 result.t1:=t2;
1872 result.t2:=t1;
1873 result.isintercept:=true;
1874 end;
1875 end;
1876 end;
1877 end;
intercept3dmynull1878 function intercept3dmy(const l1begin,l1end,l2begin,l2end:gdbvertex):intercept3dprop;
1879 var
1880 z, {t,} t1, t2, d, d1, d2: GDBDouble;
1881 begin
1882 //t := 0;
1883 //t1 := 0;
1884 //t2 := 0;
1885 result.isintercept := false;
1886 D := (l1end.y - l1begin.y) * (l2begin.x - l2end.x) - (l2begin.y - l2end.y) * (l1end.x - l1begin.x);
1887 D1 := (l1end.y - l1begin.y) * (l2begin.x - l1begin.x) - (l2begin.y - l1begin.y) * (l1end.x - l1begin.x);
1888 D2 := (l2begin.y - l1begin.y) * (l2begin.x - l2end.x) - (l2begin.y - l2end.y) * (l2begin.x - l1begin.x);
1889 if (D <> 0) then
1890 begin
1891 t1 := D1 / D;
1892 t2 := D2 / D;
1893 if ((t1 <= 1) and (t1 >= 0) and (t2 >= 0) and (t2 <= 1)) then
1894 begin
1895 result.interceptcoord.x := l1begin.x + (l1end.x - l1begin.x) * t2;
1896 result.interceptcoord.y := l1begin.y + (l1end.y - l1begin.y) * t2;
1897 result.interceptcoord.z := l1begin.z + (l1end.z - l1begin.z) * t2;
1898 z:=l2begin.z + (l2end.z - l2begin.z) * t1;
1899 if abs(result.interceptcoord.z-z)<eps then
1900 begin
1901 result.t1:=t2;
1902 result.t2:=t1;
1903 result.isintercept:=true;
1904 end;
1905 end;
1906 end;
1907 end;
intercept3dmy2null1908 function intercept3dmy2(const l1begin,l1end,l2begin,l2end:gdbvertex):intercept3dprop;
1909 var
1910 {z, t, }t1{, t2, d, d1, d2}: GDBDouble;
1911 p13,p43,p21{,pp}:gdbvertex;
1912 d1343,d4321,d1321,d4343,d2121,numer,denom:GDBDouble;
1913 begin
1914 result.isintercept := false;
1915 p13.x:= l1begin.x - l2begin.x;
1916 p13.y:= l1begin.y - l2begin.y;
1917 p13.z:= l1begin.z - l2begin.z;
1918 p43.x:= l2end.x - l2begin.x;
1919 p43.y:= l2end.y - l2begin.y;
1920 p43.z:= l2end.z - l2begin.z;
1921 if (ABS(p43.x) < EPS) and (ABS(p43.y) < EPS) and (ABS(p43.z) < EPS) then exit;
1922 p21.x:= l1end.x - l1begin.x;
1923 p21.y:= l1end.y - l1begin.y;
1924 p21.z:= l1end.z - l1begin.z;
1925 if (ABS(p21.x) < EPS) and (ABS(p21.y) < EPS) and (ABS(p21.z) < EPS) then exit;
1926
1927 d1343:= p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
1928 d4321:= p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
1929 d1321:= p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
1930 d4343:= p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
1931 d2121:= p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
1932
1933 denom:= d2121 * d4343 - d4321 * d4321;
1934 if (ABS(denom) < bigEPS) then exit;
1935 numer:= d1343 * d4321 - d1321 * d4343;
1936
1937 result.t1:=numer/denom;
1938 result.t2:= (d1343 + d4321 * result.t1) / d4343;
1939 t1:=result.t1;
1940 //t2:=result.t2;
1941
1942 {if abs(result.t1-1)<bigeps then result.t1:=1;
1943 if abs(result.t1)<bigeps then result.t1:=0;
1944 if abs(result.t2-1)<bigeps then result.t2:=1;
1945 if abs(result.t2)<bigeps then result.t2:=0;}
1946
1947 //if ((result.t1 <= 1) and (result.t1 >= 0) and (result.t2 >= 0) and (result.t2 <= 1)) then
1948 begin
1949 result.interceptcoord.x:= l1begin.x + t1 * p21.x;
1950 result.interceptcoord.y:= l1begin.y + t1 * p21.y;
1951 result.interceptcoord.z:= l1begin.z + t1 * p21.z;
1952 //pp.x:= l2begin.x + t2 * p43.x;
1953 //pp.y:= l2begin.y + t2 * p43.y;
1954 //pp.z:= l2begin.z + t2 * p43.z;
1955
1956 {if (ABS(pp.x-result.interceptcoord.x)>bigEPS) or
1957 (ABS(pp.y-result.interceptcoord.y)>bigEPS) or
1958 (ABS(pp.z-result.interceptcoord.z)>bigEPS)
1959 then exit;}
1960
1961 result.isintercept:=true;
1962 end;
1963 end;
intercept3dnull1964 function intercept3d(const l1begin,l1end,l2begin,l2end:gdbvertex):intercept3dprop;
1965 var
1966 {z, t, }t1, t2{, d, d1, d2}: GDBDouble;
1967 p13,p43,p21,pp:gdbvertex;
1968 d1343,d4321,d1321,d4343,d2121,numer,denom:GDBDouble;
1969 begin
1970 result.isintercept := false;
1971 p13.x:= l1begin.x - l2begin.x;
1972 p13.y:= l1begin.y - l2begin.y;
1973 p13.z:= l1begin.z - l2begin.z;
1974 p43.x:= l2end.x - l2begin.x;
1975 p43.y:= l2end.y - l2begin.y;
1976 p43.z:= l2end.z - l2begin.z;
1977 if (ABS(p43.x) < EPS) and (ABS(p43.y) < EPS) and (ABS(p43.z) < EPS) then exit;
1978 p21.x:= l1end.x - l1begin.x;
1979 p21.y:= l1end.y - l1begin.y;
1980 p21.z:= l1end.z - l1begin.z;
1981 if (ABS(p21.x) < EPS) and (ABS(p21.y) < EPS) and (ABS(p21.z) < EPS) then exit;
1982
1983 d1343:= p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
1984 d4321:= p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
1985 d1321:= p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
1986 d4343:= p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
1987 d2121:= p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
1988
1989 denom:= d2121 * d4343 - d4321 * d4321;
1990 if (ABS(denom) < {EPS}sqreps) then exit;
1991 numer:= d1343 * d4321 - d1321 * d4343;
1992
1993 result.t1:=numer/denom;
1994 result.t2:= (d1343 + d4321 * result.t1) / d4343;
1995 t1:=result.t1;
1996 t2:=result.t2;
1997
1998 if abs(result.t1-1)<bigeps then result.t1:=1;
1999 if abs(result.t1)<bigeps then result.t1:=0;
2000 if abs(result.t2-1)<bigeps then result.t2:=1;
2001 if abs(result.t2)<bigeps then result.t2:=0;
2002
2003 if ((result.t1 <= 1) and (result.t1 >= 0) and (result.t2 >= 0) and (result.t2 <= 1)) then
2004 begin
2005 result.interceptcoord.x:= l1begin.x + t1 * p21.x;
2006 result.interceptcoord.y:= l1begin.y + t1 * p21.y;
2007 result.interceptcoord.z:= l1begin.z + t1 * p21.z;
2008 pp.x:= l2begin.x + t2 * p43.x;
2009 pp.y:= l2begin.y + t2 * p43.y;
2010 pp.z:= l2begin.z + t2 * p43.z;
2011
2012 if (ABS(pp.x-result.interceptcoord.x)>bigEPS) or
2013 (ABS(pp.y-result.interceptcoord.y)>bigEPS) or
2014 (ABS(pp.z-result.interceptcoord.z)>bigEPS)
2015 then exit;
2016
2017 result.isintercept:=true;
2018 end;
2019 end;
2020
2021 {int LineLineIntersect(
2022 XYZ p1,XYZ p2,XYZ p3,XYZ p4,XYZ *pa,XYZ *pb,
2023 double *mua, double *mub)
2024
2025 }
2026
2027
2028
2029
2030
2031
2032
2033
2034
2035
2036
2037
2038
2039
2040
intercept2d2null2041 function intercept2d2(const x11, y11, x12, y12, x21, y21, x22, y22: GDBFloat): GDBBoolean;
2042 var
2043 {x, y, t,} t1, t2, d, d1, d2: GDBDouble;
2044 begin
2045 //x := 0;
2046 //y := 0;
2047
2048 //t := 0;
2049 //t1 := 0;
2050 //t2 := 0;
2051 result := false;
2052 D := (y12 - y11) * (x21 - x22) - (y21 - y22) * (x12 - x11);
2053 D1 := (y12 - y11) * (x21 - x11) - (y21 - y11) * (x12 - x11);
2054 D2 := (y21 - y11) * (x21 - x22) - (y21 - y22) * (x21 - x11);
2055 if (D <> 0) then
2056 begin
2057 t1 := D1 / D;
2058 t2 := D2 / D;
2059 if ((t1 <= 1) and (t1 >= 0) and (t2 >= 0) and (t2 <= 1)) then
2060 begin
2061 result := true;
2062 //x := x11 + (x12 - x11) * t2;
2063 //y := y11 + (y12 - y11) * t2;
2064 end;
2065 end;
2066 end;
vectordotnull2067 function vectordot(const v1,v2:GDBVertex):GDBVertex;
2068 begin
2069 result.x:=v1.y * v2.z - v1.z * v2.y;
2070 result.y:=v1.z * v2.x - v1.x * v2.z;
2071 result.z:=v1.x * v2.y - v1.y * v2.x;
2072 end;
scalardotnull2073 function scalardot(const v1,v2:GDBVertex):GDBDouble;
2074 begin
2075 result:=v1.x * v2.x + v1.y * v2.y +v1.z*v2.z;
2076 end;
CreateVertexnull2077 function CreateVertex(const x,y,z:GDBDouble):GDBVertex;
2078 begin
2079 result.x:=x;
2080 result.y:=y;
2081 result.z:=z;
2082 end;
CreateDoubleFromArraynull2083 function CreateDoubleFromArray(var counter:integer;const args:array of const):GDBDouble;
2084 begin
2085 case args[counter].VType of
2086 vtInteger:result:=args[counter].VInteger;
2087 vtExtended:result:=args[counter].VExtended^;
2088 end;{case}
2089 inc(counter);
2090 end;
CreateGDBStringFromArraynull2091 function CreateGDBStringFromArray(var counter:integer;const args:array of const):GDBString;
2092 begin
2093 case args[counter].VType of
2094 vtString:result:=args[counter].VString^;
2095 vtAnsiString:result:=ansistring(args[counter].VAnsiString);
2096 //vtUnicodeString:result:=args[counter].VUnicodeString^;
2097 end;{case}
2098 inc(counter);
2099 end;
CreateVertexFromArraynull2100 function CreateVertexFromArray(var counter:integer;const args:array of const):GDBVertex;
2101 var
2102 len:integer;
2103 begin
2104 len:=high(args);
2105 if (counter+2)<=(high(args)) then
2106 begin
2107 result.x:=CreateDoubleFromArray(counter,args);
2108 result.y:=CreateDoubleFromArray(counter,args);
2109 result.z:=CreateDoubleFromArray(counter,args);
2110 end
2111 else
2112 begin
2113 DebugLn('{E}CreateVertexFromArray: no enough params in args');
2114 //programlog.LogOutStr('CreateVertexFromArray: no enough params in args',lp_OldPos,LM_Error);
2115 end;
2116
2117 end;
2118
CreateVertex2Dnull2119 function CreateVertex2D(const x,y:GDBDouble):GDBVertex2D;
2120 begin
2121 result.x:=x;
2122 result.y:=y;
2123 end;
2124
2125 procedure concatBBandPoint(var fistbb:TBoundingBox;const point:GDBvertex);
2126 begin
2127 if fistbb.LBN.x>point.x then fistbb.LBN.x:=point.x;
2128 if fistbb.LBN.y>point.y then fistbb.LBN.y:=point.y;
2129 if fistbb.LBN.z>point.z then fistbb.LBN.z:=point.z;
2130
2131 if fistbb.RTF.x<point.x then fistbb.RTF.x:=point.x;
2132 if fistbb.RTF.y<point.y then fistbb.RTF.y:=point.y;
2133 if fistbb.RTF.z<point.z then fistbb.RTF.z:=point.z;
2134
2135 end;
CreateBBFrom2Pointnull2136 function CreateBBFrom2Point(const p1,p2:GDBvertex):TBoundingBox;
2137 var
2138 t,b,l,r,n,f:GDBDouble;
2139 begin
2140 if p1.x<p2.x then
2141 begin
2142 l:=p1.x;
2143 r:=p2.x;
2144 end
2145 else
2146 begin
2147 l:=p2.x;
2148 r:=p1.x;
2149 end;
2150 if p1.y<p2.y then
2151 begin
2152 b:=p1.y;
2153 t:=p2.y;
2154 end
2155 else
2156 begin
2157 b:=p2.y;
2158 t:=p1.y;
2159 end;
2160 if p1.z<p2.z then
2161 begin
2162 n:=p1.z;
2163 f:=p2.z;
2164 end
2165 else
2166 begin
2167 n:=p2.z;
2168 f:=p1.z;
2169 end;
2170 result.LBN:=CreateVertex(l,B,n);
2171 result.RTF:=CreateVertex(r,T,f);
2172 end;
CreateBBFromPointnull2173 function CreateBBFromPoint(const p:GDBvertex):TBoundingBox;
2174 begin
2175 result.LBN:=p;
2176 result.RTF:=p;
2177 end;
2178
2179 procedure ConcatBB(var fistbb:TBoundingBox;const secbb:TBoundingBox);
2180 begin
2181 if (fistbb.RTF.x=fistbb.LBN.x)
2182 and (fistbb.RTF.y=fistbb.LBN.y)
2183 and (fistbb.RTF.z=fistbb.LBN.z)
2184 then
2185 begin
2186 fistbb:=secbb;
2187 end
2188 else
2189 begin
2190 if(secbb.RTF.x=secbb.LBN.x)
2191 and (secbb.RTF.y=secbb.LBN.y)
2192 and (secbb.RTF.z=secbb.LBN.z)
2193 then
2194 else
2195 begin
2196 concatBBandPoint(fistbb,secbb.LBN);
2197 concatBBandPoint(fistbb,secbb.RTF);
2198 {concatBBandPoint(secbb,fistbb.LBN);
2199 concatBBandPoint(secbb,fistbb.RTF);}
2200 end;
2201
2202 end
2203 end;
IsBBNulnull2204 function IsBBNul(const bb:TBoundingBox):boolean;
2205 begin
2206 if (abs(bb.LBN.x-bb.RTF.x)<eps)
2207 and (abs(bb.LBN.y-bb.RTF.y)<eps)
2208 and (abs(bb.LBN.z-bb.RTF.z)<eps) then
2209 result:=true
2210 else
2211 result:=false;
2212 end;
IsPointInBBnull2213 function IsPointInBB(const point:GDBvertex; var fistbb:TBoundingBox):GDBBoolean;
2214 begin
2215 result:=false;
2216 if (fistbb.LBN.x<=point.x+eps)and(fistbb.RTF.x>=point.x-eps) then
2217 if (fistbb.LBN.y<=point.y+eps)and(fistbb.RTF.y>=point.y-eps) then
2218 if (fistbb.LBN.z<=point.z+eps)and(fistbb.RTF.z>=point.z-eps) then result:=true
2219 end;
2220 {function boundingintersect(var bb1,bb2:GDBBoundingBbox):GDBBoolean;
2221 begin
2222 result:=false;
2223 if IsPointInBB (bb1.lbn, bb2) then begin result:=true; exit end
2224 else if IsPointInBB(CreateVertex(bb1.lbn.x,bb1.rtf.y,bb1.lbn.z),bb2) then begin result:=true; exit end
2225 else if IsPointInBB(CreateVertex(bb1.rtf.x,bb1.rtf.y,bb1.lbn.z),bb2) then begin result:=true; exit end
2226 else if IsPointInBB(CreateVertex(bb1.rtf.x,bb1.lbn.y,bb1.lbn.z),bb2) then begin result:=true; exit end
2227
2228 else if IsPointInBB(CreateVertex(bb1.lbn.x,bb1.lbn.y,bb1.rtf.z),bb2) then begin result:=true; exit end
2229 else if IsPointInBB(CreateVertex(bb1.lbn.x,bb1.rtf.y,bb1.rtf.z),bb2) then begin result:=true; exit end
2230 else if IsPointInBB(CreateVertex(bb1.rtf.x,bb1.rtf.y,bb1.rtf.z),bb2) then begin result:=true; exit end
2231 else if IsPointInBB(CreateVertex(bb1.rtf.x,bb1.lbn.y,bb1.rtf.z),bb2) then begin result:=true; exit end
2232
2233
2234
2235 else if IsPointInBB (bb2.lbn, bb1) then begin result:=true; exit end
2236 else if IsPointInBB(CreateVertex(bb2.lbn.x,bb2.rtf.y,bb2.lbn.z),bb1) then begin result:=true; exit end
2237 else if IsPointInBB(CreateVertex(bb2.rtf.x,bb2.rtf.y,bb2.lbn.z),bb1) then begin result:=true; exit end
2238 else if IsPointInBB(CreateVertex(bb2.rtf.x,bb2.lbn.y,bb2.lbn.z),bb1) then begin result:=true; exit end
2239
2240 else if IsPointInBB(CreateVertex(bb2.lbn.x,bb2.lbn.y,bb2.rtf.z),bb1) then begin result:=true; exit end
2241 else if IsPointInBB(CreateVertex(bb2.lbn.x,bb2.rtf.y,bb2.rtf.z),bb1) then begin result:=true; exit end
2242 else if IsPointInBB(CreateVertex(bb2.rtf.x,bb2.rtf.y,bb2.rtf.z),bb1) then begin result:=true; exit end
2243 else if IsPointInBB(CreateVertex(bb2.rtf.x,bb2.lbn.y,bb2.rtf.z),bb1) then begin result:=true; exit end
2244
2245 end;}
boundingintersectnull2246 function boundingintersect(const bb1,bb2:TBoundingBox):GDBBoolean;
2247 var
2248 b1,b2,b1c,b2c,dist:gdbvertex;
2249 //dist:gdbdouble;
2250 begin
2251 b1.x:=(bb1.RTF.x-bb1.LBN.x)/2;
2252 b1.y:=(bb1.RTF.y-bb1.LBN.y)/2;
2253 b1.z:=(bb1.RTF.z-bb1.LBN.z)/2;
2254 b2.x:=(bb2.RTF.x-bb2.LBN.x)/2;
2255 b2.y:=(bb2.RTF.y-bb2.LBN.y)/2;
2256 b2.z:=(bb2.RTF.z-bb2.LBN.z)/2;
2257 b1c:=VertexAdd(bb1.LBN,b1);
2258 b2c:=VertexAdd(bb2.LBN,b2);
2259 dist:=VertexSub(b1c,b2c);
2260 dist.x:=abs(dist.x);
2261 dist.y:=abs(dist.y);
2262 dist.z:=abs(dist.z);
2263 result:=false;
2264 if (((b1.x+b2.x)-dist.x)>-eps)
2265 and(((b1.y+b2.y)-dist.y)>-eps)
2266 and(((b1.z+b2.z)-dist.z)>-eps) then
2267 result:=true
2268 end;
CreateMatrixFromBasisnull2269 function CreateMatrixFromBasis(const ox,oy,oz:GDBVertex):DMatrix4D;
2270 begin
2271 result:=onematrix;
2272 PGDBVertex(@result[0])^:=ox;
2273 PGDBVertex(@result[1])^:=oy;
2274 PGDBVertex(@result[2])^:=oz;
2275 end;
2276 procedure CreateBasisFromMatrix(const m:DMatrix4D;out ox,oy,oz:GDBVertex);
2277 begin
2278 ox:=PGDBVertex(@m[0])^;
2279 oy:=PGDBVertex(@m[1])^;
2280 oz:=PGDBVertex(@m[2])^;
2281 end;
QuaternionMagnitudenull2282 function QuaternionMagnitude(const q : GDBQuaternion) : GDBDouble;
2283 begin
2284 Result:=Sqrt(SqrOneVertexlength(q.ImagPart)+Sqr(q.RealPart));
2285 end;
2286 procedure NormalizeQuaternion(var q : GDBQuaternion);
2287 var
2288 m, f : GDBDouble;
2289 begin
2290 m:=QuaternionMagnitude(q);
2291 if m>EPSILON2 then begin
2292 f:=1/m;
2293 q.ImagPart:=VertexMulOnSc(q.ImagPart, f);
2294 q.RealPart:=q.RealPart*f;
2295 end else q:=IdentityQuaternion;
2296 end;
QuaternionFromMatrixnull2297 function QuaternionFromMatrix(const mat : DMatrix4D) : GDBQuaternion;
2298 // the matrix must be a rotation matrix!
2299 var
2300 traceMat, s, invS : Double;
2301 begin
2302 traceMat := 1 + mat[0,0] + mat[1,1] + mat[2,2];
2303 if traceMat>EPSILON2 then begin
2304 s:=Sqrt(traceMat)*2;
2305 invS:=1/s;
2306 Result.ImagPart.x:=(mat[1,2]-mat[2,1])*invS;
2307 Result.ImagPart.y:=(mat[2,0]-mat[0,2])*invS;
2308 Result.ImagPart.z:=(mat[0,1]-mat[1,0])*invS;
2309 Result.RealPart :=0.25*s;
2310 end else if (mat[0,0]>mat[1,1]) and (mat[0,0]>mat[2,2]) then begin // Row 0:
2311 s:=Sqrt(Max{Float}(EPSILON2, {cOne}1+mat[0,0]-mat[1,1]-mat[2,2]))*2;
2312 invS:=1/s;
2313 Result.ImagPart.x:=0.25*s;
2314 Result.ImagPart.y:=(mat[0,1]+mat[1,0])*invS;
2315 Result.ImagPart.z:=(mat[2,0]+mat[0,2])*invS;
2316 Result.RealPart :=(mat[1,2]-mat[2,1])*invS;
2317 end else if (mat[1,1]>mat[2,2]) then begin // Row 1:
2318 s:=Sqrt(Max{Float}(EPSILON2, {cOne}1+mat[1,1]-mat[0,0]-mat[2,2]))*2;
2319 invS:=1/s;
2320 Result.ImagPart.x:=(mat[0,1]+mat[1,0])*invS;
2321 Result.ImagPart.y:=0.25*s;
2322 Result.ImagPart.z:=(mat[1,2]+mat[2,1])*invS;
2323 Result.RealPart :=(mat[2,0]-mat[0,2])*invS;
2324 end else begin // Row 2:
2325 s:=Sqrt(Max{Float}(EPSILON2, {cOne}1+mat[2,2]-mat[0,0]-mat[1,1]))*2;
2326 invS:=1/s;
2327 Result.ImagPart.x:=(mat[2,0]+mat[0,2])*invS;
2328 Result.ImagPart.y:=(mat[1,2]+mat[2,1])*invS;
2329 Result.ImagPart.z:=0.25*s;
2330 Result.RealPart :=(mat[0,1]-mat[1,0])*invS;
2331 end;
2332 NormalizeQuaternion(Result);
2333 end;
QuaternionSlerpnull2334 function QuaternionSlerp(const source, dest: GDBQuaternion; const t: GDBDouble): GDBQuaternion;
2335 var
2336 to1: array[0..4] of Single;
2337 omega, cosom, sinom, scale0, scale1: Extended;
2338 // t goes from 0 to 1
2339 // absolute rotations
2340 begin
2341 // calc cosine
2342 cosom:= source.ImagPart.x*dest.ImagPart.x
2343 +source.ImagPart.y*dest.ImagPart.y
2344 +source.ImagPart.z*dest.ImagPart.z
2345 +source.RealPart *dest.RealPart;
2346 // adjust signs (if necessary)
2347 if cosom<0 then begin
2348 cosom := -cosom;
2349 to1[0] := - dest.ImagPart.x;
2350 to1[1] := - dest.ImagPart.y;
2351 to1[2] := - dest.ImagPart.z;
2352 to1[3] := - dest.RealPart;
2353 end else begin
2354 to1[0] := dest.ImagPart.x;
2355 to1[1] := dest.ImagPart.y;
2356 to1[2] := dest.ImagPart.z;
2357 to1[3] := dest.RealPart;
2358 end;
2359 // calculate coefficients
2360 if ((1.0-cosom)>EPSILON2) then begin // standard case (slerp)
2361 omega:={VectorGeometry.}ArcCos(cosom);
2362 sinom:=1/Sin(omega);
2363 scale0:=Sin((1.0-t)*omega)*sinom;
2364 scale1:=Sin(t*omega)*sinom;
2365 end else begin // "from" and "to" quaternions are very close
2366 // ... so we can do a linear interpolation
2367 scale0:=1.0-t;
2368 scale1:=t;
2369 end;
2370 // calculate final values
2371 Result.ImagPart.x := scale0 * source.ImagPart.x + scale1 * to1[0];
2372 Result.ImagPart.y := scale0 * source.ImagPart.y + scale1 * to1[1];
2373 Result.ImagPart.z := scale0 * source.ImagPart.z + scale1 * to1[2];
2374 Result.RealPart := scale0 * source.RealPart + scale1 * to1[3];
2375 NormalizeQuaternion(Result);
2376 end;
QuaternionToMatrixnull2377 function QuaternionToMatrix(quat : GDBQuaternion) : DMatrix4D;
2378 var
2379 w, x, y, z, xx, xy, xz, xw, yy, yz, yw, zz, zw: GDBDouble;
2380 begin
2381 result:=onematrix;
2382 NormalizeQuaternion(quat);
2383 w := quat.RealPart;
2384 x := quat.ImagPart.x;
2385 y := quat.ImagPart.y;
2386 z := quat.ImagPart.z;
2387 xx := x * x;
2388 xy := x * y;
2389 xz := x * z;
2390 xw := x * w;
2391 yy := y * y;
2392 yz := y * z;
2393 yw := y * w;
2394 zz := z * z;
2395 zw := z * w;
2396 Result[0, 0] := 1 - 2 * ( yy + zz );
2397 Result[1, 0] := 2 * ( xy - zw );
2398 Result[2, 0] := 2 * ( xz + yw );
2399 Result[3, 0] := 0;
2400 Result[0, 1] := 2 * ( xy + zw );
2401 Result[1, 1] := 1 - 2 * ( xx + zz );
2402 Result[2, 1] := 2 * ( yz - xw );
2403 Result[3, 1] := 0;
2404 Result[0, 2] := 2 * ( xz - yw );
2405 Result[1, 2] := 2 * ( yz + xw );
2406 Result[2, 2] := 1 - 2 * ( xx + yy );
2407 Result[3, 2] := 0;
2408 Result[0, 3] := 0;
2409 Result[1, 3] := 0;
2410 Result[2, 3] := 0;
2411 Result[3, 3] := 1;
2412 end;
GetArcParamFrom3Point2Dnull2413 function GetArcParamFrom3Point2D(Const PointData:tarcrtmodify;out ad:TArcData):GDBBoolean;
2414 var a,b,c,d,e,f,g,rr:GDBDouble;
2415 tv:gdbvertex2d;
2416 //tv3d:gdbvertex;
2417 begin
2418 A:= PointData.p2.x - PointData.p1.x;
2419 B:= PointData.p2.y - PointData.p1.y;
2420 C:= PointData.p3.x - PointData.p1.x;
2421 D:= PointData.p3.y - PointData.p1.y;
2422
2423 E:= A*(PointData.p1.x + PointData.p2.x) + B*(PointData.p1.y + PointData.p2.y);
2424 F:= C*(PointData.p1.x + PointData.p3.x) + D*(PointData.p1.y + PointData.p3.y);
2425
2426 G:= 2*(A*(PointData.p3.y - PointData.p2.y)-B*(PointData.p3.x - PointData.p2.x));
2427 if abs(g)>eps then
2428 begin
2429 result:=true;
2430 ad.p.x:= (D*E - B*F) / G;
2431 ad.p.y:= (A*F - C*E) / G;
2432 {rr}ad.r:= sqrt(sqr(PointData.p1.x - ad.p.x) + sqr(PointData.p1.y - ad.p.y));
2433 //ad.r:=rr;
2434 {Local.p_insert.x:=p_x;
2435 Local.p_insert.y:=p_y;
2436 Local.p_insert.z:=0;}
2437 tv.x:=ad.p.x;
2438 tv.y:=ad.p.y;
2439 ad.startangle:=vertexangle(tv,PointData.p1);
2440 ad.endangle:=vertexangle(tv,PointData.p3);
2441 if ad.startangle>ad.endangle then
2442 begin
2443 rr:=ad.startangle;
2444 ad.startangle:=ad.endangle;
2445 ad.endangle:=rr
2446 end;
2447 rr:=vertexangle(tv,PointData.p2);
2448 if (rr>ad.startangle) and (rr<ad.endangle) then
2449 begin
2450 end
2451 else
2452 begin
2453 rr:=ad.startangle;
2454 ad.startangle:=ad.endangle;
2455 ad.endangle:=rr
2456 end;
2457 end
2458 else
2459 result:=false;
2460 end;
2461
2462
2463 begin
2464 WorldMatrix:=oneMatrix;
2465 //CurrentCS:=OneMatrix;
2466 //wx:=@CurrentCS[0];
2467 //wy:=@CurrentCS[1];
2468 //wz:=@CurrentCS[2];
2469 //w0:=@CurrentCS[3];
2470 end.
2471