hschumann2/TempleOS-Source-Code
0847
1 2#help_index "Graphics/Math"3 4public I64 gr_x_offsets[8]={-1, 0, 1,-1,1,-1,0,1},5 gr_y_offsets[8]={-1,-1,-1, 0,0, 1,1,1},6 gr_x_offsets2[4]={ 0,-1, 1, 0},7 gr_y_offsets2[4]={-1, 0, 0, 1};8 9public Bool Line(U8 *aux_data,I64 x1,I64 y1,I64 z1,I64 x2,I64 y2,I64 z2,10 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),I64 step=1,I64 start=0)11{//Step through line segment calling callback.12//Uses fixed-point.13 I64 i,j,d,dx=x2-x1,dy=y2-y1,dz=z2-z1,_x,_y,_z,14 adx=AbsI64(dx),ady=AbsI64(dy),adz=AbsI64(dz);15 Bool first=TRUE;16 if (adx>=ady) {17 if (adx>=adz) {18 if (d=adx) {19 if (dx>=0)20 dx=0x100000000;21 else22 dx=-0x100000000;23 dy=dy<<32/d;24 dz=dz<<32/d;25 }26 } else {27 if (d=adz) {28 dx=dx<<32/d;29 dy=dy<<32/d;30 if (dz>=0)31 dz=0x100000000;32 else33 dz=-0x100000000;34 }35 }36 } else {37 if (ady>=adz) {38 if (d=ady) {39 dx=dx<<32/d;40 if (dy>=0)41 dy=0x100000000;42 else43 dy=-0x100000000;44 dz=dz<<32/d;45 }46 } else {47 if (d=adz) {48 dx=dx<<32/d;49 dy=dy<<32/d;50 if (dz>=0)51 dz=0x100000000;52 else53 dz=-0x100000000;54 }55 }56 }57 x1<<=32; y1<<=32; z1<<=32;58 for (j=0;j<start;j++) {59 x1+=dx; y1+=dy; z1+=dz;60 }61 if (step!=1 && step!=0) {62 dx*=step;63 dy*=step;64 dz*=step;65 d/=step;66 }67 for (i=start;i<=d;i++) {68 if ((_x!=x1.i32[1] || _y!=y1.i32[1] || _z!=z1.i32[1] || first) &&69 !(*fp_plot)(aux_data,x1.i32[1],y1.i32[1],z1.i32[1]))70 return FALSE;71 first=FALSE;72 _x=x1.i32[1]; _y=y1.i32[1]; _z=z1.i32[1];73 x1+=dx; y1+=dy; z1+=dz;74 }75 if (step==1 && (_x!=x2||_y!=y2||_z!=z2) && !(*fp_plot)(aux_data,x2,y2,z2))76 return FALSE;77 return TRUE;78}79 80#help_index "Graphics/Math/3D Transformation"81public I64 *Mat4x4MulMat4x4Equ(I64 *dst,I64 *m1,I64 *m2)82{//Multiply 4x4 matrices and store in dst. Uses fixed-point.83//Conceptually, the transform m1 is applied after m284 I64 i,j,k;85 F64 sum;86 for (i=0;i<4;i++) {87 for (j=0;j<4;j++) {88 sum=0;89 for (k=0;k<4;k++)90 sum+=ToF64(m1[k+4*j])*ToF64(m2[i+4*k]);91 dst[i+4*j]=sum/GR_SCALE;92 }93 }94 return dst;95}96 97public I64 *Mat4x4MulMat4x4New(I64 *m1,I64 *m2,CTask *mem_task=NULL)98{//Multiply 4x4 matrices. Return MAlloced matrix. Uses fixed-point.99//Conceptually, the transform m1 is applied after m2100 return Mat4x4MulMat4x4Equ(MAlloc(sizeof(I64)*16,mem_task),m1,m2);101}102 103public I64 *Mat4x4Equ(I64 *dst,I64 *src)104{//Copy 4x4 Rot matrix.105 MemCpy(dst,src,sizeof(I64)*16);106 return dst;107}108 109public I64 *Mat4x4New(I64 *src,CTask *mem_task=NULL)110{//Return MAlloced copy of 4x4 Rot matrix.111 return Mat4x4Equ(MAlloc(sizeof(I64)*16,mem_task),src);112}113 114public I64 *Mat4x4RotX(I64 *m,F64 phi)115{//Rot matrix about X axis. Uses fixed-point.116 F64 my_cos=Cos(phi)*GR_SCALE,my_sin=Sin(phi)*GR_SCALE;117 I64 r[16],r2[16];118 MemSet(r,0,sizeof(r));119 r[5]=my_cos;120 r[10]=my_cos;121 r[9]=my_sin;122 r[6]=-my_sin;123 r[0]=GR_SCALE;124 r[15]=GR_SCALE;125 return Mat4x4Equ(m,Mat4x4MulMat4x4Equ(r2,r,m));126}127 128public I64 *Mat4x4RotY(I64 *m,F64 omega)129{//Rot matrix about Y axis. Uses fixed-point.130 F64 my_cos=Cos(omega)*GR_SCALE,my_sin=Sin(omega)*GR_SCALE;131 I64 r[16],r2[16];132 MemSet(r,0,sizeof(r));133 r[0]=my_cos;134 r[10]=my_cos;135 r[8]=-my_sin;136 r[2]=my_sin;137 r[5]=GR_SCALE;138 r[15]=GR_SCALE;139 return Mat4x4Equ(m,Mat4x4MulMat4x4Equ(r2,r,m));140}141 142public I64 *Mat4x4RotZ(I64 *m,F64 theta)143{//Rot matrix about Z axis. Uses fixed-point.144 F64 my_cos=Cos(theta)*GR_SCALE,my_sin=Sin(theta)*GR_SCALE;145 I64 r[16],r2[16];146 MemSet(r,0,sizeof(r));147 r[0]=my_cos;148 r[5]=my_cos;149 r[4]=my_sin;150 r[1]=-my_sin;151 r[10]=GR_SCALE;152 r[15]=GR_SCALE;153 return Mat4x4Equ(m,Mat4x4MulMat4x4Equ(r2,r,m));154}155 156public I64 *Mat4x4Scale(I64 *m,F64 s)157{//Scale 4x4 matrix by value.158 I64 i;159 for (i=0;i<16;i++)160 m[i]*=s;161 return m;162}163 164public U0 DCThickScale(CDC *dc=gr.dc)165{//Scale device context's thick by norm of transformation.166 I64 d;167 if (dc->flags&DCF_TRANSFORMATION) {168 if (dc->thick) {169 d=dc->thick*dc->r_norm+0x80000000; //Round170 dc->thick=d.i32[1];171 if (dc->thick<1)172 dc->thick=1;173 }174 }175}176 177public I64 *Mat4x4TranslationEqu(I64 *r,I64 x,I64 y,I64 z)178{//Set translation values in 4x4 matrix. Uses fixed-point.179 r[0*4+3]=x<<32;180 r[1*4+3]=y<<32;181 r[2*4+3]=z<<32;182 r[3*4+3]=GR_SCALE;183 return r;184}185 186public I64 *Mat4x4TranslationAdd(I64 *r,I64 x,I64 y,I64 z)187{//Add translation to 4x4 matrix. Uses fixed-point.188 r[0*4+3]+=x<<32;189 r[1*4+3]+=y<<32;190 r[2*4+3]+=z<<32;191 r[3*4+3]=GR_SCALE;192 return r;193}194 195public Bool DCSymmetrySet(CDC *dc=gr.dc,I64 x1,I64 y1,I64 x2,I64 y2)196{//2D. Set device context's symmetry.197 F64 d;198 if (y1==y2 && x1==x2)199 return FALSE;200 dc->sym.snx=y2-y1;201 dc->sym.sny=x1-x2;202 dc->sym.snz=0;203 if (d=Sqrt(SqrI64(dc->sym.snx)+204 SqrI64(dc->sym.sny)+205 SqrI64(dc->sym.snz))) {206 d=GR_SCALE/d;207 dc->sym.snx *= d;208 dc->sym.sny *= d;209 dc->sym.snz *= d;210 }211 dc->sym.sx=x1;212 dc->sym.sy=y1;213 dc->sym.sz=0;214 return TRUE;215}216 217public Bool DCSymmetry3Set(CDC *dc=gr.dc,I64 x1,I64 y1,I64 z1,218 I64 x2,I64 y2,I64 z2,I64 x3,I64 y3,I64 z3)219{//3D. Set device context's symmetry.220 F64 d,x,y,z,xx,yy,zz;221 I64 xx1,yy1,zz1,xx2,yy2,zz2,*r;222 xx1=x1-x2; yy1=y1-y2; zz1=z1-z2;223 xx2=x3-x2; yy2=y3-y2; zz2=z3-z2;224 if (!xx1 && !yy1 && !zz1 ||225 !xx2 && !yy2 && !zz2 ||226 xx1==xx2 && yy1==yy2 && zz1==zz2)227 return FALSE;228 229 x=yy1*zz2-zz1*yy2;230 y=-xx1*zz2+zz1*xx2;231 z=xx1*yy2-yy1*xx2;232 if (dc->flags & DCF_TRANSFORMATION) {233 r=dc->r;234 xx=x*r[0]+y*r[1]+z*r[2];235 yy=x*r[4]+y*r[5]+z*r[6];236 zz=x*r[8]+y*r[9]+z*r[10];237 x=xx; y=yy; z=zz;238 }239 if (d=Sqrt(Sqr(x)+Sqr(y)+Sqr(z))) {240 d=GR_SCALE/d;241 dc->sym.snx = d*x;242 dc->sym.sny = d*y;243 dc->sym.snz = d*z;244 }245 if (dc->flags & DCF_TRANSFORMATION)246 (*dc->transform)(dc,&x1,&y1,&z1);247 dc->sym.sx=x1;248 dc->sym.sy=y1;249 dc->sym.sz=z1;250 return TRUE;251}252 253public U0 DCReflect(CDC *dc,I64 *_x,I64 *_y,I64 *_z)254{//Reflect 3D point about device context's symmetry. Uses fixed-point.255 I64 x=*_x<<32,y=*_y<<32,z=*_z<<32,256 xx=*_x-dc->sym.sx,yy=*_y-dc->sym.sy,zz=*_z-dc->sym.sz,257 d=(xx*dc->sym.snx+yy*dc->sym.sny+zz*dc->sym.snz)>>16,258 xn,yn,zn,xx2,yy2,zz2;259 xn=d*dc->sym.snx>>15;260 yn=d*dc->sym.sny>>15;261 zn=d*dc->sym.snz>>15;262 xx=x-xn;263 yy=y-yn;264 zz=z-zn;265 xx2=x+xn;266 yy2=y+yn;267 zz2=z+zn;268 if (SqrI64(xx>>16 -dc->sym.sx<<16)+269 SqrI64(yy>>16 -dc->sym.sy<<16)+270 SqrI64(zz>>16 -dc->sym.sz<<16)<271 SqrI64(xx2>>16-dc->sym.sx<<16)+272 SqrI64(yy2>>16-dc->sym.sy<<16)+273 SqrI64(zz2>>16-dc->sym.sz<<16)) {274 *_x=xx.i32[1]; *_y=yy.i32[1]; *_z=zz.i32[1];275 } else {276 *_x=xx2.i32[1]; *_y=yy2.i32[1]; *_z=zz2.i32[1];277 }278}279 280#help_index "Graphics/Math"281#define GR_SCALE1_BITS 24282#define GR_SCALE2_BITS 8283public Bool Circle(U8 *aux_data,I64 cx,I64 cy,I64 cz,I64 radius,284 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),285 I64 step=1,F64 start_radians=0,F64 len_radians=2*pi)286{//Step through circle arc calling callback.287 I64 i,j,len=Ceil(len_radians*radius),288 x,y,x1,y1,s1,s2,c;289 F64 t;290 if (radius<=0||!step) return TRUE;291 t=1.0/radius;292 c=1<<GR_SCALE1_BITS*Cos(t);293 if (step<0) {294 step=-step;295 s2=1<<GR_SCALE1_BITS*Sin(t);296 s1=-s2;297 } else {298 s1=1<<GR_SCALE1_BITS*Sin(t);299 s2=-s1;300 }301 if (start_radians) {302 x=radius*Cos(start_radians);303 y=-radius*Sin(start_radians);304 } else {305 x=radius;306 y=0;307 }308 x<<=GR_SCALE2_BITS;309 y<<=GR_SCALE2_BITS;310 for (i=0;i<=len;i+=step) {311 if (!(*fp_plot)(aux_data,cx+x>>GR_SCALE2_BITS,cy+y>>GR_SCALE2_BITS,cz))312 return FALSE;313 for (j=0;j<step;j++) {314 x1=(c*x+s1*y)>>GR_SCALE1_BITS;315 y1=(s2*x+c*y)>>GR_SCALE1_BITS;316 x=x1; y=y1;317 }318 }319 return TRUE;320}321 322public Bool Ellipse(U8 *aux_data,I64 cx,I64 cy,I64 cz,323 I64 x_radius,I64 y_radius,Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),324 F64 rot_angle=0,I64 step=1,F64 start_radians=0,F64 len_radians=2*pi)325{//Step through ellipse arc calling callback.326 I64 i,j,len,327 x,y,_x,_y,x1,y1,x2,y2, s1,s2,c, s12,s22,c2;328 F64 t;329 Bool first=TRUE;330 if (x_radius<=0 || y_radius<=0 || !step)331 return TRUE;332 if (x_radius>=y_radius) {333 t=1.0/x_radius;334 len=Ceil(len_radians*x_radius);335 } else {336 t=1.0/y_radius;337 len=Ceil(len_radians*y_radius);338 }339 340 c=1<<GR_SCALE1_BITS*Cos(t);341 if (step<0) {342 step=-step;343 s2=1<<GR_SCALE1_BITS*Sin(t);344 s1=-s2;345 } else {346 s1=1<<GR_SCALE1_BITS*Sin(t);347 s2=-s1;348 }349 350 c2=1<<GR_SCALE1_BITS*Cos(rot_angle);351 s12=1<<GR_SCALE1_BITS*Sin(rot_angle);352 s22=-s12;353 354 if (start_radians) {355 x=x_radius*Cos(start_radians);356 y=-x_radius*Sin(start_radians);357 } else {358 x=x_radius;359 y=0;360 }361 x<<=GR_SCALE2_BITS;362 y<<=GR_SCALE2_BITS;363 x2=x;364 y2=y;365 366 y1=y2*y_radius/x_radius;367 x=(c2*x2+s12*y1)>>GR_SCALE1_BITS;368 y=(s22*x2+c2*y1)>>GR_SCALE1_BITS;369 370 for (i=0;i<=len;i+=step) {371 if ((x>>GR_SCALE2_BITS!=_x || y>>GR_SCALE2_BITS!=_y || first) &&372 !(*fp_plot)(aux_data,cx+x>>GR_SCALE2_BITS,cy+y>>GR_SCALE2_BITS,cz))373 return FALSE;374 375 _x=x>>GR_SCALE2_BITS; _y=y>>GR_SCALE2_BITS; first=FALSE;376 for (j=0;j<step;j++) {377 x1=(c*x2+s1*y2)>>GR_SCALE1_BITS;378 y1=(s2*x2+c*y2)>>GR_SCALE1_BITS;379 x2=x1;380 y2=y1;381 y1=y1*y_radius/x_radius;382 x=(c2*x1+s12*y1)>>GR_SCALE1_BITS;383 y=(s22*x1+c2*y1)>>GR_SCALE1_BITS;384 }385 }386 return TRUE;387}388 389public Bool RegPoly(U8 *aux_data,I64 cx,I64 cy,I64 cz,390 I64 x_radius,I64 y_radius,I64 sides,391 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),392 F64 rot_angle=0,I64 step=1,F64 start_radians=0,F64 len_radians=2*pi)393{//Step through regular polygon calling callback.394 I64 i,n,x,y,x1,y1,x2,y2,395 xx1,yy1,xx2,yy2,396 s1,s2,c, s12,s22,c2;397 F64 angle_step;398 399 if (sides<=0 || x_radius<=0 || y_radius<=0)400 return TRUE;401 402 angle_step=2*pi/sides;403 n=len_radians*sides/(2*pi);404 405 s1=1<<GR_SCALE1_BITS*Sin(angle_step);406 s2=-s1;407 c=1<<GR_SCALE1_BITS*Cos(angle_step);408 409 s12=1<<GR_SCALE1_BITS*Sin(rot_angle);410 s22=-s12;411 c2=1<<GR_SCALE1_BITS*Cos(rot_angle);412 413 if (start_radians) {414 x=x_radius*Cos(start_radians);415 y=-x_radius*Sin(start_radians);416 } else {417 x=x_radius;418 y=0;419 }420 x<<=GR_SCALE2_BITS;421 y<<=GR_SCALE2_BITS;422 x2=x;423 y2=y;424 425 y1=y2*y_radius/x_radius;426 x=(c2*x2+s12*y1)>>GR_SCALE1_BITS;427 y=(s22*x2+c2*y1)>>GR_SCALE1_BITS;428 429 xx1=cx+x>>GR_SCALE2_BITS;430 yy1=cy+y>>GR_SCALE2_BITS;431 for (i=0;i<n;i++) {432 x1=(c*x2+s1*y2)>>GR_SCALE1_BITS;433 y1=(s2*x2+c*y2)>>GR_SCALE1_BITS;434 x2=x1;435 y2=y1;436 y1=y1*y_radius/x_radius;437 x=(c2*x1+s12*y1)>>GR_SCALE1_BITS;438 y=(s22*x1+c2*y1)>>GR_SCALE1_BITS;439 xx2=cx+x>>GR_SCALE2_BITS;440 yy2=cy+y>>GR_SCALE2_BITS;441 if (!Line(aux_data,xx1,yy1,cz,xx2,yy2,cz,fp_plot,step))442 return FALSE;443 xx1=xx2; yy1=yy2;444 }445 return TRUE;446}447 448#help_index "Graphics/Data Types/D3I32;Math/Data Types/D3I32;Data Types/D3I32"449public F64 D3I32Dist(CD3I32 *p1,CD3I32 *p2)450{//Distance451 return Sqrt(SqrI64(p1->x-p2->x)+SqrI64(p1->y-p2->y)+SqrI64(p1->z-p2->z));452}453 454public I64 D3I32DistSqr(CD3I32 *p1,CD3I32 *p2)455{//Distance Squared456 return SqrI64(p1->x-p2->x)+SqrI64(p1->y-p2->y)+SqrI64(p1->z-p2->z);457}458 459public F64 D3I32Norm(CD3I32 *p)460{//Norm461 return Sqrt(SqrI64(p->x)+SqrI64(p->y)+SqrI64(p->z));462}463 464public I64 D3I32NormSqr(CD3I32 *p)465{//Norm Squared466 return SqrI64(p->x)+SqrI64(p->y)+SqrI64(p->z);467}468 469#help_index "Graphics/Math"470public Bool Bezier2(U8 *aux_data,CD3I32 *ctrl,471 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),Bool first=TRUE)472{//Go in 2nd order bezier calling callback.473 I64 x,y,z,xx,yy,zz,dx,dy,dz,d_max;474 F64 x0=ctrl[0].x,y0=ctrl[0].y,z0=ctrl[0].z,475 x1=ctrl[1].x-x0,y1=ctrl[1].y-y0,z1=ctrl[1].z-z0,476 x2=ctrl[2].x-x0,y2=ctrl[2].y-y0,z2=ctrl[2].z-z0,477 t,d=D3I32Dist(&ctrl[0],&ctrl[1])+478 D3I32Dist(&ctrl[1],&ctrl[2])+479 D3I32Dist(&ctrl[2],&ctrl[0]),480 s=0.5/d,t1,t2;481 xx=x0; yy=y0; zz=z0;482 if (first && !(*fp_plot)(aux_data,xx,yy,zz))483 return FALSE;484 for (t=0.0;t<=1.0;t+=s) {485 t1=t*(1.0-t);486 t2=t*t;487 x=x0+x1*t1+x2*t2;488 y=y0+y1*t1+y2*t2;489 z=z0+z1*t1+z2*t2;490 dx=AbsI64(x-xx);491 dy=AbsI64(y-yy);492 dz=AbsI64(z-zz);493 if (dx>dy)494 d_max=dx;495 else496 d_max=dy;497 if (dz>d_max)498 d_max=dz;499 if (!d_max)500 s*=1.1;501 else {502 s*=0.9;503 if (!(*fp_plot)(aux_data,x,y,z))504 return FALSE;505 xx=x;yy=y;zz=z;506 }507 }508 x=ctrl[2].x; y=ctrl[2].y; z=ctrl[2].z;509 if ((xx!=x || yy!=y || zz!=z) &&510 !(*fp_plot)(aux_data,x,y,z))511 return FALSE;512 return TRUE;513}514 515public Bool Bezier3(U8 *aux_data,CD3I32 *ctrl,516 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),Bool first=TRUE)517{//Go in 3rd order bezier calling callback.518 I64 x,y,z,xx,yy,zz,dx,dy,dz,d_max;519 F64 x0=ctrl[0].x,y0=ctrl[0].y,z0=ctrl[0].z,520 x1=ctrl[1].x-x0,y1=ctrl[1].y-y0,z1=ctrl[1].z-z0,521 x2=ctrl[2].x-x0,y2=ctrl[2].y-y0,z2=ctrl[2].z-z0,522 x3=ctrl[3].x-x0,y3=ctrl[3].y-y0,z3=ctrl[3].z-z0,523 t,d=D3I32Dist(&ctrl[0],&ctrl[1])+524 D3I32Dist(&ctrl[1],&ctrl[2])+525 D3I32Dist(&ctrl[2],&ctrl[3])+526 D3I32Dist(&ctrl[3],&ctrl[0]),527 s=0.5/d,nt,t1,t2,t3;528 xx=x0; yy=y0; zz=z0;529 if (first && !(*fp_plot)(aux_data,xx,yy,zz))530 return FALSE;531 for (t=0.0;t<=1.0;t+=s) {532 nt=1.0-t;533 t1=t*nt*nt;534 t2=t*t*nt;535 t3=t*t*t;536 x=x0+x1*t1+x2*t2+x3*t3;537 y=y0+y1*t1+y2*t2+y3*t3;538 z=z0+z1*t1+z2*t2+z3*t3;539 dx=AbsI64(x-xx);540 dy=AbsI64(y-yy);541 dz=AbsI64(z-zz);542 if (dx>dy)543 d_max=dx;544 else545 d_max=dy;546 if (dz>d_max)547 d_max=dz;548 if (!d_max)549 s*=1.1;550 else {551 s*=0.9;552 if (!(*fp_plot)(aux_data,x,y,z))553 return FALSE;554 xx=x;yy=y;zz=z;555 }556 }557 x=ctrl[3].x; y=ctrl[3].y; z=ctrl[3].z;558 if ((xx!=x || yy!=y || zz!=z) &&559 !(*fp_plot)(aux_data,x,y,z))560 return FALSE;561 return TRUE;562}563 564public Bool BSpline2(U8 *aux_data,CD3I32 *ctrl,I64 cnt,565 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),Bool closed=FALSE)566{//Go in 2nd order spline calling callback.567 I64 i,j;568 CD3I32 *c;569 Bool first;570 if (cnt<3) return FALSE;571 first=TRUE;572 if (closed) {573 cnt++;574 c=MAlloc(sizeof(CD3I32)*(cnt*2-1));575 j=1;576 for (i=0;i<cnt-2;i++) {577 c[j].x=(ctrl[i].x+ctrl[i+1].x)/2.0;578 c[j].y=(ctrl[i].y+ctrl[i+1].y)/2.0;579 c[j].z=(ctrl[i].z+ctrl[i+1].z)/2.0;580 j+=2;581 }582 c[j].x=(ctrl[0].x+ctrl[cnt-2].x)/2.0;583 c[j].y=(ctrl[0].y+ctrl[cnt-2].y)/2.0;584 c[j].z=(ctrl[0].z+ctrl[cnt-2].z)/2.0;585 586 c[0].x=(c[1].x+c[j].x)/2.0;587 c[0].y=(c[1].y+c[j].y)/2.0;588 c[0].z=(c[1].z+c[j].z)/2.0;589 j=2;590 for (i=0;i<cnt-2;i++) {591 c[j].x=(c[j-1].x+c[j+1].x)/2.0;592 c[j].y=(c[j-1].y+c[j+1].y)/2.0;593 c[j].z=(c[j-1].z+c[j+1].z)/2.0;594 j+=2;595 }596 c[j].x=c[0].x;597 c[j].y=c[0].y;598 c[j].z=c[0].z;599 } else {600 c=MAlloc(sizeof(CD3I32)*(cnt*2-1));601 c[0].x=ctrl[0].x;602 c[0].y=ctrl[0].y;603 c[0].z=ctrl[0].z;604 c[cnt*2-2].x=ctrl[cnt-1].x;605 c[cnt*2-2].y=ctrl[cnt-1].y;606 c[cnt*2-2].z=ctrl[cnt-1].z;607 j=1;608 for (i=0;i<cnt-1;i++) {609 c[j].x=(ctrl[i].x+ctrl[i+1].x)/2.0;610 c[j].y=(ctrl[i].y+ctrl[i+1].y)/2.0;611 c[j].z=(ctrl[i].z+ctrl[i+1].z)/2.0;612 j+=2;613 }614 j=2;615 for (i=0;i<cnt-2;i++) {616 c[j].x=(c[j-1].x+c[j+1].x)/2.0;617 c[j].y=(c[j-1].y+c[j+1].y)/2.0;618 c[j].z=(c[j-1].z+c[j+1].z)/2.0;619 j+=2;620 }621 }622 for (i=0;i<cnt*2-2;i+=2) {623 if (!Bezier2(aux_data,&c[i],fp_plot,first))624 return FALSE;625 first=FALSE;626 }627 Free(c);628 return TRUE;629}630 631public Bool BSpline3(U8 *aux_data,CD3I32 *ctrl,I64 cnt,632 Bool (*fp_plot)(U8 *aux,I64 x,I64 y,I64 z),Bool closed=FALSE)633{//Go in 3rd order spline calling callback.634 I64 i,j;635 F64 x,y,z;636 CD3I32 *c;637 Bool first;638 if (cnt<3) return FALSE;639 first=TRUE;640 if (closed) {641 cnt++;642 c=MAlloc(sizeof(CD3I32)*(cnt*3-2));643 j=1;644 for (i=0;i<cnt-2;i++) {645 x=ctrl[i].x;646 y=ctrl[i].y;647 z=ctrl[i].z;648 c[j].x=(ctrl[i+1].x-x)/3.0+x;649 c[j].y=(ctrl[i+1].y-y)/3.0+y;650 c[j].z=(ctrl[i+1].z-z)/3.0+z;651 j++;652 c[j].x=2.0*(ctrl[i+1].x-x)/3.0+x;653 c[j].y=2.0*(ctrl[i+1].y-y)/3.0+y;654 c[j].z=2.0*(ctrl[i+1].z-z)/3.0+z;655 j+=2;656 }657 x=ctrl[cnt-2].x;658 y=ctrl[cnt-2].y;659 z=ctrl[cnt-2].z;660 c[j].x=(ctrl[0].x-x)/3.0+x;661 c[j].y=(ctrl[0].y-y)/3.0+y;662 c[j].z=(ctrl[0].z-z)/3.0+z;663 j++;664 c[j].x=2.0*(ctrl[0].x-x)/3.0+x;665 c[j].y=2.0*(ctrl[0].y-y)/3.0+y;666 c[j].z=2.0*(ctrl[0].z-z)/3.0+z;667 668 c[0].x=(c[1].x+c[j].x)/2.0;669 c[0].y=(c[1].y+c[j].y)/2.0;670 c[0].z=(c[1].z+c[j].z)/2.0;671 672 j=3;673 for (i=0;i<cnt-2;i++) {674 c[j].x=(c[j-1].x+c[j+1].x)/2.0;675 c[j].y=(c[j-1].y+c[j+1].y)/2.0;676 c[j].z=(c[j-1].z+c[j+1].z)/2.0;677 j+=3;678 }679 c[j].x=c[0].x;680 c[j].y=c[0].y;681 c[j].z=c[0].z;682 } else {683 c=MAlloc(sizeof(CD3I32)*(cnt*3-2));684 c[0].x=ctrl[0].x;685 c[0].y=ctrl[0].y;686 c[0].z=ctrl[0].z;687 c[cnt*3-3].x=ctrl[cnt-1].x;688 c[cnt*3-3].y=ctrl[cnt-1].y;689 c[cnt*3-3].z=ctrl[cnt-1].z;690 j=1;691 for (i=0;i<cnt-1;i++) {692 x=ctrl[i].x;693 y=ctrl[i].y;694 z=ctrl[i].z;695 c[j].x=(ctrl[i+1].x-x)/3.0+x;696 c[j].y=(ctrl[i+1].y-y)/3.0+y;697 c[j].z=(ctrl[i+1].z-z)/3.0+z;698 j++;699 c[j].x=2.0*(ctrl[i+1].x-x)/3.0+x;700 c[j].y=2.0*(ctrl[i+1].y-y)/3.0+y;701 c[j].z=2.0*(ctrl[i+1].z-z)/3.0+z;702 j+=2;703 }704 j=3;705 for (i=0;i<cnt-2;i++) {706 c[j].x=(c[j-1].x+c[j+1].x)/2.0;707 c[j].y=(c[j-1].y+c[j+1].y)/2.0;708 c[j].z=(c[j-1].z+c[j+1].z)/2.0;709 j+=3;710 }711 }712 for (i=0;i<cnt*3-3;i+=3) {713 if (!Bezier3(aux_data,&c[i],fp_plot,first))714 return FALSE;715 first=FALSE;716 }717 Free(c);718 return TRUE;719}720 721#define CC_LEFT 1722#define CC_RIGHT 2723#define CC_TOP 4724#define CC_BOTTOM 8725 726public Bool ClipLine(I64 *_x1,I64 *_y1,I64 *_x2,I64 *_y2,727 I64 left,I64 top,I64 right,I64 bottom)728{//Clip x1,y1 x2,y2 with left,top,right,bottom.729 I64 x,y,x1=*_x1,y1=*_y1,x2=*_x2,y2=*_y2,730 cc,cc1,cc2;731 if (y1>bottom)732 cc1=CC_BOTTOM;733 else if (y1<top)734 cc1=CC_TOP;735 else736 cc1=0;737 if (x1>right)738 cc1|=CC_RIGHT;739 else if (x1<left)740 cc1|=CC_LEFT;741 742 if (y2>bottom)743 cc2=CC_BOTTOM;744 else if (y2<top)745 cc2=CC_TOP;746 else747 cc2=0;748 if (x2>right)749 cc2|=CC_RIGHT;750 else if (x2<left)751 cc2|=CC_LEFT;752 753 while (TRUE) {754 if (!(cc1|cc2))755 return TRUE;756 if (cc1&cc2)757 return FALSE;758 759 if (cc1)760 cc=cc1;761 else762 cc=cc2;763 764 if (cc&CC_BOTTOM) {765 x=x1+(x2-x1)*(bottom-y1)/(y2-y1);766 y=bottom;767 } else if (cc&CC_TOP) {768 x=x1+(x2-x1)*(top-y1)/(y2-y1);769 y=top;770 } else if (cc&CC_RIGHT) {771 y=y1+(y2-y1)*(right-x1)/(x2-x1);772 x=right;773 } else {774 y=y1+(y2-y1)*(left-x1)/(x2-x1);775 x=left;776 }777 778 if (cc==cc1) {779 *_x1=x1=x;780 *_y1=y1=y;781 if (y1>bottom)782 cc1=CC_BOTTOM;783 else if (y1<top)784 cc1=CC_TOP;785 else786 cc1=0;787 if (x1>right)788 cc1|=CC_RIGHT;789 else if (x1<left)790 cc1|=CC_LEFT;791 } else {792 *_x2=x2=x;793 *_y2=y2=y;794 if (y2>bottom)795 cc2=CC_BOTTOM;796 else if (y2<top)797 cc2=CC_TOP;798 else799 cc2=0;800 if (x2>right)801 cc2|=CC_RIGHT;802 else if (x2<left)803 cc2|=CC_LEFT;804 }805 }806}807