z3d.c (13194B)
1 /* 18aug04abu 2 * (c) Software Lab. Alexander Burger 3 */ 4 5 #include "pico.h" 6 7 #define SCL 1000000.0 8 9 typedef struct {double x, y, z;} vector; 10 typedef struct {vector a, b, c;} matrix; 11 12 static bool Snap; 13 static int SnapD, Snap1h, Snap1v, Snap2h, Snap2v; 14 static double FocLen, PosX, PosY, PosZ, Pos6, Pos9, SnapX, SnapY, SnapZ; 15 static double Coeff1, Coeff2, Coeff4, Coeff5, Coeff6, Coeff7, Coeff8, Coeff9; 16 17 18 static any getVector(any lst, vector *dst) { 19 dst->x = numToDouble(car(lst)) / SCL, lst = cdr(lst); 20 dst->y = numToDouble(car(lst)) / SCL, lst = cdr(lst); 21 dst->z = numToDouble(car(lst)) / SCL; 22 return cdr(lst); 23 } 24 25 static any putVector(vector *src, any lst) { 26 car(lst) = doubleToNum(src->x * SCL), lst = cdr(lst); 27 car(lst) = doubleToNum(src->y * SCL), lst = cdr(lst); 28 car(lst) = doubleToNum(src->z * SCL); 29 return cdr(lst); 30 } 31 32 static any getMatrix(any lst, matrix *dst) { 33 return getVector(getVector(getVector(lst, &dst->a), &dst->b), &dst->c); 34 } 35 36 static any putMatrix(matrix *src, any lst) { 37 return putVector(&src->c, putVector(&src->b, putVector(&src->a, lst))); 38 } 39 40 static void xrot(matrix *p, double ca, double sa) { 41 matrix m = *p; 42 43 p->b.x = ca * m.b.x - sa * m.c.x; 44 p->b.y = ca * m.b.y - sa * m.c.y; 45 p->b.z = ca * m.b.z - sa * m.c.z; 46 p->c.x = sa * m.b.x + ca * m.c.x; 47 p->c.y = sa * m.b.y + ca * m.c.y; 48 p->c.z = sa * m.b.z + ca * m.c.z; 49 } 50 51 // (z3d:Xrot 'angle 'model) -> T 52 any Xrot(any ex) { 53 any x; 54 double a; 55 matrix m; 56 57 a = evDouble(ex, x = cdr(ex)) / SCL; 58 x = EVAL(cadr(x)); 59 Touch(ex,x); 60 x = cdddr(val(x)); 61 getMatrix(x, &m), xrot(&m, cos(a), sin(a)), putMatrix(&m, x); 62 return T; 63 } 64 65 static void yrot(matrix *p, double ca, double sa) { 66 matrix m = *p; 67 68 p->a.x = ca * m.a.x + sa * m.c.x; 69 p->a.y = ca * m.a.y + sa * m.c.y; 70 p->a.z = ca * m.a.z + sa * m.c.z; 71 p->c.x = ca * m.c.x - sa * m.a.x; 72 p->c.y = ca * m.c.y - sa * m.a.y; 73 p->c.z = ca * m.c.z - sa * m.a.z; 74 } 75 76 // (z3d:Yrot 'angle 'model) -> T 77 any Yrot(any ex) { 78 any x; 79 double a; 80 matrix m; 81 82 a = evDouble(ex, x = cdr(ex)) / SCL; 83 x = EVAL(cadr(x)); 84 Touch(ex,x); 85 x = cdddr(val(x)); 86 getMatrix(x, &m), yrot(&m, cos(a), sin(a)), putMatrix(&m, x); 87 return T; 88 } 89 90 static void zrot(matrix *p, double ca, double sa) { 91 matrix m = *p; 92 93 p->a.x = ca * m.a.x + sa * m.b.x; 94 p->a.y = ca * m.a.y + sa * m.b.y; 95 p->a.z = ca * m.a.z + sa * m.b.z; 96 p->b.x = ca * m.b.x - sa * m.a.x; 97 p->b.y = ca * m.b.y - sa * m.a.y; 98 p->b.z = ca * m.b.z - sa * m.a.z; 99 } 100 101 // (z3d:Zrot 'angle 'model) -> T 102 any Zrot(any ex) { 103 any x; 104 double a; 105 matrix m; 106 107 a = evDouble(ex, x = cdr(ex)) / SCL; 108 x = EVAL(cadr(x)); 109 Touch(ex,x); 110 x = cdddr(val(x)); 111 getMatrix(x, &m), zrot(&m, cos(a), sin(a)), putMatrix(&m, x); 112 return T; 113 } 114 115 // (z3d:Arot 'angle 'model) -> T 116 any Arot(any ex) { 117 any x; 118 double a, n; 119 matrix m; 120 vector pt; 121 122 a = evDouble(ex, x = cdr(ex)) / SCL; 123 x = EVAL(cadr(x)); 124 Touch(ex,x); 125 x = cdddr(val(x)); 126 getVector(cddar(getMatrix(x, &m)), &pt); 127 n = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z); 128 pt.x /= n, pt.y /= n, pt.z /= n; // Axis unit vector 129 if ((n = sqrt(pt.y*pt.y + pt.z*pt.z)) == 0.0) // Axis parallel to x-axis 130 a *= pt.x, xrot(&m, cos(a), sin(a)); 131 else { 132 xrot(&m, pt.z/n, -pt.y/n); 133 yrot(&m, n, pt.x); 134 zrot(&m, cos(a), sin(a)); 135 yrot(&m, n, -pt.x); 136 xrot(&m, pt.z/n, pt.y/n); 137 } 138 putMatrix(&m, x); 139 return T; 140 } 141 142 // (z3d:Rotate 'X 'Y 'Z 'model 'varX 'varY 'varZ ['flg]) -> T 143 any Rotate(any ex) { 144 any x; 145 double vx, vy, vz; 146 matrix m; 147 cell c1, c2, c3; 148 149 vx = evDouble(ex, x = cdr(ex)) / SCL; 150 vy = evDouble(ex, x = cdr(x)) / SCL; 151 vz = evDouble(ex, x = cdr(x)) / SCL; 152 x = cdr(x), getMatrix(cdddr(val(EVAL(car(x)))), &m); 153 x = cdr(x), Push(c1, EVAL(car(x))); 154 NeedVar(ex,data(c1)); 155 x = cdr(x), Push(c2, EVAL(car(x))); 156 NeedVar(ex,data(c2)); 157 x = cdr(x), Push(c3, EVAL(car(x))); 158 NeedVar(ex,data(c3)); 159 if (isNil(EVAL(cadr(x)))) { 160 if (!isNil(data(c1))) 161 val(data(c1)) = doubleToNum((vx * m.a.x + vy * m.b.x + vz * m.c.x) * SCL); 162 if (!isNil(data(c2))) 163 val(data(c2)) = doubleToNum((vx * m.a.y + vy * m.b.y + vz * m.c.y) * SCL); 164 if (!isNil(data(c3))) 165 val(data(c3)) = doubleToNum((vx * m.a.z + vy * m.b.z + vz * m.c.z) * SCL); 166 } 167 else { 168 if (!isNil(data(c1))) 169 val(data(c1)) = doubleToNum((vx * m.a.x + vy * m.a.y + vz * m.a.z) * SCL); 170 if (!isNil(data(c2))) 171 val(data(c2)) = doubleToNum((vx * m.b.x + vy * m.b.y + vz * m.b.z) * SCL); 172 if (!isNil(data(c3))) 173 val(data(c3)) = doubleToNum((vx * m.c.x + vy * m.c.y + vz * m.c.z) * SCL); 174 } 175 drop(c1); 176 return T; 177 } 178 179 static void _approach(any ex, double d, any dst, any src) { 180 any l1, l2; 181 int i; 182 double n; 183 184 Touch(ex,dst); 185 l1 = val(dst); 186 Fetch(ex,src); 187 l2 = val(src); 188 for (i = 0; i < 12; ++i) { 189 n = numToDouble(car(l1)) / SCL; 190 car(l1) = doubleToNum((n + d * (numToDouble(car(l2)) / SCL - n)) * SCL); 191 l1 = cdr(l1), l2 = cdr(l2); 192 } 193 do { 194 while (!isSym(car(l1))) 195 if (!isCell(l1 = cdr(l1))) 196 return; 197 while (!isSym(car(l2))) 198 if (!isCell(l2 = cdr(l2))) 199 return; 200 _approach(ex, d, car(l1), car(l2)); 201 } while (isCell(l1 = cdr(l1)) && isCell(l2 = cdr(l2))); 202 } 203 204 // (z3d:Approach 'num 'model 'model) -> T 205 any Approach(any ex) { 206 any x; 207 long n; 208 cell c1, c2; 209 210 n = evCnt(ex, x = cdr(ex)); 211 x = cdr(x), Push(c1, EVAL(car(x))); 212 x = cdr(x), Push(c2, EVAL(car(x))); 213 _approach(ex, 1.0 / (double)n, data(c1), data(c2)); 214 drop(c1); 215 return T; 216 } 217 218 // (z3d:Spot 'dx 'dy 'dz ['x 'y 'z]) -> (yaw . pitch) 219 any Spot(any ex) { 220 any x; 221 double dx, dy, dz; 222 cell c1; 223 224 dx = evDouble(ex, x = cdr(ex)) / SCL; 225 dy = evDouble(ex, x = cdr(x)) / SCL; 226 dz = evDouble(ex, x = cdr(x)) / SCL; 227 228 if (isCell(x = cdr(x))) { 229 dx -= evDouble(ex, x) / SCL; 230 dy -= evDouble(ex, x = cdr(x)) / SCL; 231 dz -= evDouble(ex, x = cdr(x)) / SCL; 232 } 233 234 Push(c1, doubleToNum(atan2(dy,dx) * SCL)); 235 dx = sqrt(dx*dx + dy*dy + dz*dz); 236 data(c1) = cons(data(c1), doubleToNum(dx==0.0? 0.0 : asin(dz/dx)*SCL)); 237 return Pop(c1); 238 } 239 240 static void rotate(vector *src, matrix *p, vector *dst) { 241 dst->x = src->x * p->a.x + src->y * p->b.x + src->z * p->c.x; 242 dst->y = src->x * p->a.y + src->y * p->b.y + src->z * p->c.y; 243 dst->z = src->x * p->a.z + src->y * p->b.z + src->z * p->c.z; 244 } 245 246 #if 0 247 /* (lst -- x y z) */ 248 void Locate(void) { 249 any lst; 250 vector pos, v, w; 251 matrix rot, r; 252 253 lst = Tos; 254 getMatrix(getVector(car(lst), &pos), &rot); 255 while (isCell(lst = cdr(lst))) { 256 getMatrix(getVector(car(lst), &v), &r); 257 rotate(&v, &rot, &w); 258 pos.x += w.x, pos.y += w.y, pos.z += w.z; 259 v = r.a, rotate(&v, &rot, &r.a); 260 v = r.b, rotate(&v, &rot, &r.b); 261 v = r.c, rotate(&v, &rot, &r.c); 262 rot = r; 263 } 264 Tos = doubleToNum(pos.x) * SCL; 265 push(doubleToNum(pos.y)) * SCL; 266 push(doubleToNum(pos.z)) * SCL; 267 } 268 #endif 269 270 static void shadowPt(double vx, double vy) { 271 double z; 272 273 z = Coeff7 * vx + Coeff8 * vy - Pos9; 274 prn((int)(FocLen * (Coeff1 * vx + Coeff2 * vy) / z)); 275 prn((int)(FocLen * (Coeff4 * vx + Coeff5 * vy - Pos6) / z)); 276 prn(num(1000.0 * z)); 277 } 278 279 static void transPt(double vx, double vy, double vz) { 280 double x, y, z; 281 int h, v, dh, dv, d; 282 283 x = Coeff1 * vx + Coeff2 * vy; 284 y = Coeff4 * vx + Coeff5 * vy + Coeff6 * vz; 285 z = Coeff7 * vx + Coeff8 * vy + Coeff9 * vz; 286 prn(h = (int)(FocLen * x/z)); 287 prn(v = (int)(FocLen * y/z)); 288 prn(num(1000.0 * z)); 289 if (Snap) { 290 if ((dh = h - Snap1h) < 0) 291 dh = -dh; 292 if ((dv = v - Snap1v) < 0) 293 dv = -dv; 294 if ((d = dh>dv? dh+dv*41/100-dh/24 : dv+dh*41/100-dv/24) < SnapD) { 295 SnapD = d; 296 Snap2h = h; Snap2v = v; 297 SnapX = vx; SnapY = vy; SnapZ = vz; 298 } 299 } 300 } 301 302 static void doDraw(any ex, any mdl, matrix *r, double x, double y, double z) { 303 any face, c1, c2, txt; 304 long n, pix; 305 double dx, dy, dz; 306 vector pos, pt1, pt2, pt3, v, w, nv; 307 matrix rot; 308 309 Fetch(ex,mdl); 310 mdl = getMatrix(getVector(val(mdl), &pos), &rot); 311 if (!r) 312 r = &rot; 313 else { 314 v = pos, rotate(&v, r, &pos); 315 pos.x += x, pos.y += y, pos.z += z; 316 v = rot.a, rotate(&v, r, &rot.a); 317 v = rot.b, rotate(&v, r, &rot.b); 318 v = rot.c, rotate(&v, r, &rot.c); 319 } 320 dx = pos.x - PosX; 321 dy = pos.y - PosY; 322 dz = pos.z - PosZ; 323 324 if ((z = Coeff7*dx + Coeff8*dy + Coeff9*dz) < 0.1) 325 return; 326 if (z < fabs(Coeff1*dx + Coeff2*dy)) 327 return; 328 if (z < fabs(Coeff4*dx + Coeff5*dy + Coeff6*dz)) 329 return; 330 331 while (isCell(mdl)) { 332 face = car(mdl), mdl = cdr(mdl); 333 if (isSym(face)) 334 doDraw(ex, face, &rot, pos.x, pos.y, pos.z); 335 else { 336 c1 = car(face), face = cdr(face); 337 c2 = car(face), face = cdr(face); 338 if (!isSym(car(face))) 339 txt = Nil; 340 else 341 txt = car(face), face = cdr(face); 342 face = getVector(getVector(face, &v), &w); 343 if ((v.x || v.y || v.z) && (w.x || w.y || w.z)) 344 r = &rot, rotate(&v, r, &pt1), rotate(&w, r, &pt2); 345 else 346 rotate(&v, r, &pt1), rotate(&w, r, &pt2), r = &rot; 347 face = getVector(face, &v), rotate(&v, r, &pt3); 348 if (c2 == T) { 349 n = length(face) / 3; 350 prn(n+2); 351 shadowPt(pt1.x + dx + pt1.z + pos.z, pt1.y + dy); 352 pr(txt); 353 shadowPt(pt2.x + dx + pt2.z + pos.z, pt2.y + dy); 354 shadowPt(pt3.x + dx + pt3.z + pos.z, pt3.y + dy); 355 while (--n >= 0) { 356 face = getVector(face, &v), rotate(&v, r, &pt1); 357 shadowPt(pt1.x + dx + pt1.z + pos.z, pt1.y + dy); 358 } 359 prn(0); 360 } 361 else { 362 v.x = pt1.x - pt2.x; 363 v.y = pt1.y - pt2.y; 364 v.z = pt1.z - pt2.z; 365 w.x = pt3.x - pt2.x; 366 w.y = pt3.y - pt2.y; 367 w.z = pt3.z - pt2.z; 368 nv.x = v.y * w.z - v.z * w.y; 369 nv.y = v.z * w.x - v.x * w.z; 370 nv.z = v.x * w.y - v.y * w.x; 371 pt1.x += dx, pt1.y += dy, pt1.z += dz; 372 if (isNil(c1) && isNil(c2)) 373 pix = -1; // Transparent 374 else { 375 if (pt1.x * nv.x + pt1.y * nv.y + pt1.z * nv.z >= 0.0) { 376 if (isNil(c1)) 377 continue; // Backface culling 378 pix = unDig(c1) / 2; 379 n = 80 - num(14.14 * (nv.z-nv.x) / sqrt(nv.x*nv.x + nv.y*nv.y + nv.z*nv.z)); 380 } 381 else { 382 if (isNil(c2)) 383 continue; // Backface culling 384 pix = unDig(c2) / 2; 385 n = 80 + num(14.14 * (nv.z-nv.x) / sqrt(nv.x*nv.x + nv.y*nv.y + nv.z*nv.z)); 386 } 387 pix = ((pix >> 16) & 255) * n / 100 << 16 | 388 ((pix >> 8) & 255) * n / 100 << 8 | (pix & 255) * n / 100; 389 } 390 n = length(face) / 3; 391 prn(n+2); 392 transPt(pt1.x, pt1.y, pt1.z); 393 pr(txt); 394 transPt(pt2.x + dx, pt2.y + dy, pt2.z + dz); 395 transPt(pt3.x + dx, pt3.y + dy, pt3.z + dz); 396 while (--n >= 0) { 397 face = getVector(face, &v), rotate(&v, r, &pt1); 398 transPt(pt1.x + dx, pt1.y + dy, pt1.z + dz); 399 } 400 prn(pix); 401 } 402 } 403 } 404 } 405 406 // (z3d:Draw 'foc 'yaw 'pitch 'x 'y 'z 'sky 'gnd ['h 'v]) -> NIL 407 // (z3d:Draw 'sym) -> NIL 408 // (z3d:Draw 'NIL) -> lst 409 any Draw(any ex) { 410 any x, y; 411 double a, sinY, cosY, sinP, cosP; 412 413 x = cdr(ex); 414 if (isNil(y = EVAL(car(x)))) { 415 cell c1; 416 417 prn(0); 418 if (!Snap) { 419 prn(32767); 420 return Nil; 421 } 422 prn(Snap2h), prn(Snap2v); 423 Push(c1, doubleToNum(SnapZ * SCL)); 424 data(c1) = cons(doubleToNum(SnapY * SCL), data(c1)); 425 data(c1) = cons(doubleToNum(SnapX * SCL), data(c1)); 426 return Pop(c1); 427 } 428 if (isSym(y)) { 429 doDraw(ex, y, NULL, 0.0, 0.0, 0.0); 430 return Nil; 431 } 432 FocLen = numToDouble(y) / SCL; 433 a = evDouble(ex, x = cdr(x)) / SCL, sinY = sin(a), cosY = cos(a); 434 a = evDouble(ex, x = cdr(x)) / SCL, sinP = sin(a), cosP = cos(a); 435 PosX = evDouble(ex, x = cdr(x)) / SCL; 436 PosY = evDouble(ex, x = cdr(x)) / SCL; 437 PosZ = evDouble(ex, x = cdr(x)) / SCL; 438 439 Coeff1 = -sinY; 440 Coeff2 = cosY; 441 Coeff4 = cosY * sinP; 442 Coeff5 = sinY * sinP; 443 Coeff6 = -cosP; 444 Coeff7 = cosY * cosP; 445 Coeff8 = sinY * cosP; 446 Coeff9 = sinP; 447 448 Pos6 = Coeff6 * PosZ; 449 Pos9 = Coeff9 * PosZ; 450 451 if (cosP == 0.0) 452 prn(sinP > 0.0? +16383 : -16384); 453 else if ((a = FocLen * sinP/cosP) > +16383.0) 454 prn(+16383); 455 else if (a < -16384.0) 456 prn(-16384); 457 else 458 prn(num(a)); 459 prn(evCnt(ex, x = cdr(x))); 460 prn(evCnt(ex, x = cdr(x))); 461 x = cdr(x); 462 if (Snap = !isNil(y = EVAL(car(x)))) { 463 SnapD = 32767; 464 Snap1h = (int)xCnt(ex,y); 465 Snap1v = (int)evCnt(ex,cdr(x)); 466 } 467 return Nil; 468 }