Home My Page Projects Code Snippets Project Openings SML/NJ
 Summary Activity Forums Tracker Lists Tasks Docs Surveys News SCM Files

# SCM Repository

[smlnj] Annotation of /sml/trunk/benchmarks/todo/pia/pia.sml
 [smlnj] / sml / trunk / benchmarks / todo / pia / pia.sml

# Annotation of /sml/trunk/benchmarks/todo/pia/pia.sml

Revision 193 - (view) (download)

 1 : monnier 193 structure Main : sig val doit : unit -> unit end = 2 : struct 3 : (* Copyright 1989 by AT&T Bell Laboratories *) 4 : structure Fastlib = struct 5 : 6 : structure Ref = 7 : struct 8 : open Ref 9 : fun inc r = r := !r + 1 10 : fun dec r = r := !r - 1 11 : end 12 : 13 : structure List : LIST = 14 : struct 15 : open List 16 : fun hd (a::r) = a | hd nil = raise Hd 17 : fun tl (a::r) = r | tl nil = raise Tl 18 : fun null nil = true | null _ = false 19 : fun length l = 20 : let fun j(k,nil) = k 21 : | j(k, a::x) = j(k+1,x) 22 : in j(0,l) 23 : end 24 : fun op @ (nil,l) = l 25 : | op @ (a::r, l) = a :: (r@l) 26 : fun rev l = 27 : let fun f (nil, h) = h 28 : | f (a::r, h) = f(r, a::h) 29 : in f(l,nil) 30 : end 31 : fun map f = 32 : let fun m nil = nil 33 : | m (a::r) = f a :: m r 34 : in m 35 : end 36 : fun fold f [] = (fn b => b) 37 : | fold f (a::r) = (fn b => let fun f2(e,[]) = f(e,b) 38 : | f2(e,a::r) = f(e,f2(a,r)) 39 : in f2(a,r) 40 : end) 41 : fun revfold f [] = (fn b => b) 42 : | revfold f (a::r) = (fn b => let fun f2(e,[],b) = f(e,b) 43 : | f2(e,a::r,b) = f2(a,r,f(e,b)) 44 : in f2(a,r,b) 45 : end) 46 : fun app f = let fun a2 (e::r) = (f e; a2 r) | a2 nil = () in a2 end 47 : fun revapp f = let fun a2 (e::r) = (a2 r; f e; ()) | a2 nil = () in a2 end 48 : fun nthtail(e,0) = e 49 : | nthtail(e::r,n) = nthtail(r,n-1) 50 : | nthtail _ = raise NthTail 51 : fun nth x = hd(nthtail x) handle NthTail => raise Nth | Hd => raise Nth 52 : fun exists pred = 53 : let fun f nil = false 54 : | f (hd::tl) = pred hd orelse f tl 55 : in f 56 : end 57 : end 58 : 59 : structure ByteArray (* : BYTEARRAY *) = 60 : struct 61 : open ByteArray 62 : local open System.Unsafe 63 : val slength:bytearray -> int = cast slength 64 : val store:bytearray * int * int -> unit = cast store 65 : val ordof:bytearray * int -> int = cast ordof 66 : in 67 : val length = slength 68 : fun update(arg as (s,i,c)) = 69 : if i<0 orelse i >= slength s then raise Subscript 70 : else if c<0 orelse c>255 then raise Range 71 : else store arg 72 : val op sub = fn (s,i) => if i<0 orelse i>= slength s then raise Subscript 73 : else ordof(s,i) 74 : fun extract(ba,i,1) : string = 75 : if i<0 orelse i >= slength ba then raise Subscript 76 : else cast ordof(ba,i) 77 : | extract(s,i,len) = 78 : if i<0 orelse i+len > slength s orelse len<0 then raise Subscript 79 : else if len=0 then cast Assembly.bytearray0 80 : else let val a = Assembly.A.create_b len 81 : fun copy j = if j=len then () 82 : else (store(a,j,ordof(s,i+j)); copy(j+1)) 83 : in copy 0; cast a 84 : end 85 : fun app f ba = 86 : let val len = slength ba 87 : fun app' i = if i >= len then () 88 : else (f(ordof(ba,i)); app'(i+1)) 89 : in app' 0 90 : end 91 : fun revapp f ba = 92 : let fun revapp' i = if i < 0 then () 93 : else (f(ordof(ba,i)); revapp'(i-1)) 94 : in revapp'(slength ba - 1) 95 : end 96 : fun fold f ba x = 97 : let fun fold'(i,x) = if i < 0 then x else fold'(i-1, f(ordof(ba,i),x)) 98 : in fold'(slength ba - 1, x) 99 : end 100 : fun revfold f ba x = 101 : let val len = slength ba 102 : fun revfold'(i,x) = if i >= len then x 103 : else revfold'(i+1,f(ordof(ba,i),x)) 104 : in revfold'(0,x) 105 : end 106 : end 107 : end 108 : 109 : structure String = 110 : struct 111 : open String 112 : local open System.Unsafe 113 : val op > = Integer.> and op >= = Integer.>= 114 : val op < = Integer.< and op <= = Integer.<= 115 : in 116 : fun length s = if boxed s then slength s else 1 117 : 118 : val size = length 119 : fun substring("",0,0) = "" (* never call create_s with 0 *) 120 : | substring("",_,_) = raise Substring 121 : | substring(s,i,0) = if i>=0 122 : then if boxed s then if i <= slength s 123 : then "" else raise Substring 124 : else if i<=1 125 : then "" else raise Substring 126 : else raise Substring 127 : | substring(s,0,1) = if boxed s then cast(ordof(s,0)) else s 128 : | substring(s,i,1) = 129 : if boxed s then if i>=0 andalso i < slength s 130 : then cast(ordof(s,i)) 131 : else raise Substring 132 : else if i=0 then s else raise Substring 133 : | substring(s,i,len) = 134 : if boxed s andalso i>=0 andalso i+len <= slength s 135 : andalso len >= 0 136 : then let val a = Assembly.A.create_s(len) 137 : fun copy j = if j=len then () 138 : else (store(a,j,ordof(s,i+j)); copy(j+1)) 139 : in copy 0; a 140 : end 141 : else raise Substring 142 : 143 : fun explode s = 144 : if boxed s 145 : then let fun f(l,~1) = l 146 : | f(l, i) = f(cast(ordof(s,i)) :: l, i-1) 147 : in f(nil, slength s - 1) 148 : end 149 : else [s] 150 : fun op ^ ("",s) = s 151 : | op ^ (s,"") = s 152 : | op ^ (x,y) = 153 : if boxed x 154 : then if boxed y 155 : then let val xl = slength x and yl = slength y 156 : val a = Assembly.A.create_s(xl+yl) 157 : fun copyx n = if n=xl then () 158 : else (store(a,n,ordof(x,n)); copyx(n+1)) 159 : fun copyy n = if n=yl then () 160 : else (store(a,xl+n,ordof(y,n)); copyy(n+1)) 161 : in copyx 0; copyy 0; a 162 : end 163 : else let val xl = slength x 164 : val a = Assembly.A.create_s(xl+1) 165 : fun copyx n = if n=xl then () 166 : else (store(a,n,ordof(x,n)); copyx(n+1)) 167 : in copyx 0; store(a,xl,cast y); a 168 : end 169 : else if boxed y 170 : then let val yl = slength y 171 : val a = Assembly.A.create_s(1+yl) 172 : fun copyy n = if n=yl then () 173 : else (store(a,1+n,ordof(y,n)); copyy(n+1)) 174 : in store(a,0,cast x); copyy 0; a 175 : end 176 : else let val a = Assembly.A.create_s 2 177 : in store(a,0,cast x); store(a,1,cast y); a 178 : end 179 : fun chr i = if i<0 orelse i>255 then raise Chr else cast i 180 : fun ord "" = raise Ord 181 : | ord s = if boxed s then ordof(s,0) else cast s 182 : val ordof = fn (s,i) => 183 : if boxed s 184 : then if i<0 orelse i>= slength s then raise Ord else ordof(s,i) 185 : else if i=0 then cast s else raise Ord 186 : fun implode (sl:string list) = 187 : let val len = List.fold(fn(s,l) => length s + l) sl 0 188 : in case len 189 : of 0 => "" 190 : | 1 => let fun find (""::tl) = find tl 191 : | find (hd::_) = cast hd 192 : | find nil = "" (* impossible *) 193 : in find sl 194 : end 195 : | _ => let val new = Assembly.A.create_s len 196 : fun copy (nil,_) = () 197 : | copy (s::tl,base) = 198 : let val len = length s 199 : fun copy0 0 = () 200 : | copy0 i = 201 : let val next = i-1 202 : in store(new,base+next,ordof(s,next)); 203 : copy0 next 204 : end 205 : in copy0 len; 206 : copy(tl,base+len) 207 : end 208 : in copy(sl,0); 209 : new 210 : end 211 : end 212 : end (* local *) 213 : end (* structure String *) 214 : 215 : structure General = 216 : struct 217 : open General 218 : fun f o g = fn x => f(g x) 219 : fun a before b = a 220 : end (* structure General *) 221 : 222 : structure Array = 223 : struct 224 : open Array 225 : local open System.Unsafe in 226 : val op sub : 'a array * int -> 'a = 227 : fn (a,i) => 228 : if i<0 orelse i >= length a then raise Subscript 229 : else subscript(a,i) 230 : val update : 'a array * int * 'a -> unit = 231 : fn (a,i,v) => 232 : if i<0 orelse i >= length a then raise Subscript 233 : else update(a,i,v) 234 : end (* local open ... *) 235 : end (* structure Array *) 236 : 237 : structure Integer = 238 : struct 239 : open Integer 240 : fun op rem(a:int,b:int):int = a-((a quot b) * b) 241 : fun min(a,b) = if ab then a else b 243 : fun abs a = if a<0 then ~a else a 244 : end 245 : 246 : val inc = Ref.inc 247 : val dec = Ref.dec 248 : val hd = List.hd and tl = List.tl 249 : val null = List.null and length = List.length 250 : val op @ = List.@ and rev = List.rev 251 : val map = List.map and fold = List.fold and revfold=List.revfold 252 : val app = List.app and revapp = List.revapp 253 : val nthtail = List.nthtail and nth = List.nth and exists = List.exists 254 : val substring = String.substring and explode = String.explode 255 : val op ^ = String.^ and chr = String.chr and ord = String.ord 256 : val implode=String.implode 257 : val op o = General.o and op before = General.before 258 : val op sub = Array.sub and update= Array.update 259 : val min = Integer.min and max = Integer.max 260 : 261 : end 262 : open Fastlib; 263 : 264 : (* FILE: Build.PIA *) 265 : fun query_prompt () = output(std_out, 266 : "\n================QUERY================\n")(*;*) 267 : 268 : (* ask user if timing code is required *) 269 : val timing_wanted = true 270 : (** 271 : let val ans = (query_prompt(); 272 : output(std_out,"\nTiming code required, (y/n) ?\n"); 273 : input(std_in,1) 274 : ) 275 : in 276 : (ans = "y") orelse (ans = "Y") 277 : end(*;*) 278 : **) 279 : 280 : (* load the system *) 281 : (* val _ = use "LIBRARY/extra_maths.sml"; *) 282 : (*==================================================================*) 283 : (* a collection of maths functions *) 284 : (* mosly obvious *) 285 : 286 : 287 : 288 : 289 : (*==================================================================*) 290 : (* MATHS *) 291 : (* ===== *) 292 : 293 : fun sign (N:real) = if (N<0.0) then (~1.0) else (1.0)(*;*) 294 : fun round (N:real) = floor(N+0.5)(*;*) 295 : fun ceil x = if x<1.0 then 0 else 1+ceil(x-1.0)(*;*) 296 : 297 : fun sqr (n:real) = n*n(*;*) 298 : fun cube (n:real) = n * n * n(*;*) 299 : 300 : fun max ((x:real),(y:real)) = 301 : if x > y then x else y(*;*) 302 : fun min ((x:real),(y:real)) = 303 : if x > y then y else x(*;*) 304 : 305 : fun real_zero (x:real) = (* accept that reals need care :-) *) 306 : (abs x) < 1E~15(*;*) 307 : fun real_equal (x:real) y = 308 : if x = y then true 309 : else if x+y = 0.0 then false 310 : else abs((x-y)/(x+y)) < 1E~15(*;*) 311 : 312 : fun int_equal (x:int) y = (x=y)(*;*) 313 : 314 : fun odd n = n mod 2 <> 0(*;*) 315 : exception e_real_odd(*;*) 316 : fun real_odd n = if real_zero (n - real(floor n)) 317 : then not( real_zero (n - (real(floor (n/2.0))*2.0)) ) 318 : else raise e_real_odd (*;*) 319 : 320 : (* atan2 returns pi/2 for infinity *) (* check with Patrick *) 321 : (* atan2 returns 0.0 for 0.0/0.0 *) 322 : fun atan2(b,a) = 323 : if (real_zero b andalso real_zero a ) then 0.0 324 : else if (real_zero a ) andalso (b>0.0) then 1.570796326794897 325 : else if (real_zero a ) andalso (b<0.0) then ~1.570796326794897 326 : else if (real_zero b ) andalso (a>0.0) then 0.0 327 : else if (real_zero b ) andalso (a<0.0) then 3.141598163 328 : else if (a>0.0) andalso (b>0.0) then arctan(b/a) 329 : else if (a<0.0) andalso (b<0.0) then arctan(b/a) - 3.141598163 330 : else if (a<0.0) andalso (b>0.0) then arctan(b/a) + 3.141598163 331 : else arctan(b/a)(*;*) 332 : 333 : fun rad_to_deg (x:real) = (180.0 / 3.141598163) * x(*;*) 334 : 335 : exception e_power(*;*) 336 : fun pow x y = if real_zero x then 0.0 (* x to a real power *) 337 : else if x<0.0 then raise e_power 338 : else if real_zero y then 1.0 339 : else exp( y* (ln x))(*;*) 340 : 341 : fun cube_root x = 342 : if x <0.0 343 : then (~(pow (~x) (1.0/3.0) ) ) 344 : else (pow x (1.0/3.0) )(*;*) 345 : 346 : fun sign_pow x y = (* x to a real signed power *) 347 : if x < 0.0 348 : then if (real_odd y) 349 : then ( ~(pow (~x) y) ) 350 : else (pow (~x) y) 351 : else ( pow x y )(*;*) 352 : 353 : 354 : (*==================================================================*) 355 : 356 : 357 : (* val _ = use "LIBRARY/IO_prims"; *) 358 : (* primitives for simple IO taken from Wikstrom Appendix D *) 359 : exception e_have(*;*) 360 : exception e_getint(*;*) 361 : exception e_digitval(*;*) 362 : 363 : fun digit c = c>= "0" andalso c<= "9"(*;*) 364 : 365 : fun digitval d = if digit d then ord d - ord "0" else raise e_digitval(*;*) 366 : 367 : fun skip s = if lookahead s = " " orelse 368 : lookahead s = "\n" orelse 369 : lookahead s = "\t" 370 : then (input(s,1);skip s) 371 : else ()(*;*) 372 : 373 : fun have s c = if c= lookahead s then input(s,1) 374 : else (output(std_out,"Did not get "^ c); 375 : raise e_have)(*;*) 376 : local fun getint' s n = 377 : if digit(lookahead s) 378 : then getint' s (10*n+digitval( input(s,1) ) ) 379 : else n 380 : fun getposint s = if digit(lookahead s) 381 : then getint' s 0 382 : else raise e_getint 383 : in fun getint s = 384 : (skip s; if lookahead s = "~" orelse 385 : lookahead s = "-" 386 : then ( input(s,1) ; ~(getposint s)) 387 : else getposint s) 388 : and get_no_skip_int s = 389 : (if lookahead s = "~" orelse 390 : lookahead s = "-" 391 : then ( input(s,1) ; ~(getposint s)) 392 : else getposint s) 393 : 394 : end(*;*) 395 : 396 : 397 : exception early_eof; 398 : fun safe_get_int (s:instream) = 399 : if end_of_stream s then raise early_eof 400 : else getint s(*;*) 401 : 402 : 403 : (*==================================================================*) 404 : local 405 : exception e_notreal 406 : fun getsimreal' (s:instream) (n:real) (c:int)= 407 : if digit(lookahead s) 408 : then getsimreal' s (n+(real(digitval( input(s,1) 409 : ))/(pow 10.0 (real c))) ) (c+1) 410 : else n 411 : in 412 : fun getsimreal (s:instream) = 413 : let val intpart = (skip s; 414 : if lookahead s = "." then 0.0 415 : else (real(get_no_skip_int s)) ) 416 : in 417 : if lookahead s = "." 418 : then 419 : ( input(s,1) ; (* get rid of decimal point *) 420 : if intpart >= 0.0 421 : then intpart+(getsimreal' s 0.0 1) 422 : else intpart-(getsimreal' s 0.0 1) 423 : ) 424 : else 425 : if lookahead s = " " orelse 426 : lookahead s = "\n" orelse 427 : lookahead s = "\t" 428 : then (* no decimal point so return intpart *) 429 : intpart 430 : else 431 : raise e_notreal 432 : end 433 : end(*;*) 434 : 435 : 436 : fun int_to_string 0 = "0" 437 : | int_to_string (N:int) = int_to_string (N div 10) ^ chr (ord "0" + N mod 10)(*;*) 438 : 439 : 440 : local fun convert 0 = "" 441 : | convert n = convert(n div 10) ^ chr(ord "0" + n mod 10) 442 : in fun putint s 0 = output(s,"0") 443 : | putint s n = 444 : if n<0 then ( output(s,"-"); 445 : putint s (~n)) 446 : else output(s,convert n) 447 : end(*;*) 448 : 449 : 450 : local fun getchars s = if lookahead s = "\"" then "" 451 : else input(s,1)^getchars s 452 : in fun getstring s = (skip s; have s "\""; 453 : let val str = getchars s 454 : in (have s "\""; str) end) 455 : end(*;*) 456 : 457 : 458 : local fun get_no_space_string (file:instream) = 459 : if lookahead file = " " orelse 460 : lookahead file = "\t" orelse 461 : lookahead file = "\n" 462 : then ("") 463 : else input(file,1)^(get_no_space_string file); 464 : in 465 : fun get_unquoted_string (file:instream) = (skip file; 466 : get_no_space_string file); 467 : end(*;*) 468 : 469 : 470 : 471 : fun putstring s str = 472 : output(s,str)(*;*) 473 : 474 : fun skip_to_eol s = 475 : if lookahead s = "\n" 476 : then ( input(s,1) ;()) 477 : else if end_of_stream s then () 478 : else ( input(s,1) ; skip_to_eol s) (*;*) 479 : 480 : 481 : fun read_to_eol s= 482 : if lookahead s = "\n" orelse 483 : lookahead s = "" 484 : then "" 485 : else 486 : (input(s ,1))^read_to_eol s(*;*) 487 : 488 : 489 : local 490 : fun putleadzerosint s _ 0 = () 491 : | putleadzerosint s Num (Width:int) = 492 : let val digit = Num div floor (pow 10.0 (real (Width-1))) 493 : val reminder = Num - (digit * floor ( pow 10.0 (real(Width-1)))) 494 : in 495 : (putint s digit; 496 : putleadzerosint s reminder (Width-1); 497 : () 498 : ) 499 : end 500 : 501 : fun split (N:real) Width= 502 : ( floor N, floor( ( N - (real (floor N)) )* (pow 10.0 (real(Width))) )) 503 : in 504 : fun putreal s (R:real) (Width:int)= 505 : let val (Whole,Part) = split R Width 506 : in 507 : if R < 0.0 508 : then 509 : (putstring s "-"; 510 : putreal s (~R) Width 511 : ) 512 : else 513 : (putint s Whole; 514 : putstring s "."; 515 : putleadzerosint s Part Width ) 516 : end 517 : end(*;*) 518 : 519 : fun putreality s (N:real) = putreal s N 9(*;*) 520 : 521 : 522 : 523 : fun get_filename s = (* get_filename *) 524 : (output (std_out,"\n" ^ s ^ "\n"); 525 : skip std_in; 526 : read_to_eol std_in )(*;*) 527 : 528 : 529 : (* val _ = use "LIBRARY/first_things.sml"; *) 530 : (* Things I expect to have available *) 531 : 532 : (*=================================================================*) 533 : (* file descriptors *) 534 : (* ================ *) 535 : 536 : (* screen keyboard files *) 537 : val FILEIN = std_in(*;*) 538 : val FILEOUT = std_out(*;*) 539 : 540 : (*=================================================================*) 541 : (* a collection of useful types *) 542 : (* ============================ *) 543 : 544 : type PTS_2 = real*real(*;*) 545 : type PTS_3 = real*real*real(*;*) 546 : 547 : (*=================================================================*) 548 : (* a collection of useful values *) 549 : (* ============================= *) 550 : 551 : val origin = (0.0,0.0,0.0)(*;*) 552 : 553 : 554 : (*=================================================================*) 555 : (* a collection of useful functions *) 556 : (* ================================ *) 557 : 558 : (* GENERAL *) 559 : fun curry f = fn x => fn y => f(x,y)(*;*) (* f x y => f(x,y) *) 560 : fun uncurry f = fn(x,y) => f x y(*;*) (* f(x,y) => f x y *) 561 : 562 : (* LIST *) 563 : exception e_combine(*;*) 564 : fun combine (nil,nil) = nil (* list x, list y => list(x,y) *) 565 : | combine ( (x :: xs),(y::ys) )=( (x,y)::combine(xs,ys)) 566 : | combine (_,_) = raise e_combine(*;*) 567 : 568 : fun append Head Tail = Head @ Tail(*;*) 569 : 570 : fun uncurryappend (Head,Tail) = append Head Tail(*;*) 571 : 572 : fun member _ nil = false 573 : | member A (B::C) = if A = B then true 574 : else member A C(*;*) 575 : 576 : exception ith_empty(*;*) 577 : fun ith (I:int) nil = raise ith_empty (* get ith element of list *) 578 : | ith 1 (a::rest) = a 579 : | ith N (a::rest) = ith (N-1) rest(*;*) 580 : 581 : 582 : fun len nil = 0 583 : | len (_::rest) = 1 + (len rest)(*;*) 584 : 585 : fun all nil = true (* true of all x are true *) 586 : | all (x::xs) = x andalso (all xs)(*;*) 587 : 588 : fun distribute a nil = nil (* ???? *) 589 : | distribute a (b::c) = 590 : ([b,a] :: ( [a,b] :: (distribute a c)) )(*;*) 591 : 592 : fun permute nil = nil (* ???? *) 593 : | permute (a::b) = 594 : ( distribute a b) @ (permute b)(*;*) 595 : 596 : fun merge(List,nil) = List (* set merge not ordered *) 597 : | merge(List1,List2) = 598 : let val head = hd List2 599 : val tail = tl List2 600 : in 601 : if member head List1 602 : then 603 : merge(List1,tail) 604 : else 605 : merge(head::List1,tail) 606 : end(*;*) 607 : 608 : fun curry_merge L1 L2 = merge(L1,L2)(*;*) 609 : 610 : fun flatten nil = nil (* list of list => list *) 611 : | flatten (xs::xss) = xs @ (flatten xss)(*;*) 612 : 613 : fun reverse [] = [] 614 : | reverse (a::rest) = append (reverse rest) [a](*;*) 615 : 616 : fun rev (al:'a list) = reverse al(*;*) 617 : 618 : fun gen_rev_i_list 0 = [] (* generate n N element list [1...N] *) 619 : | gen_rev_i_list N = N::(gen_rev_i_list (N-1))(*;*) 620 : fun gen_i_list N = reverse (gen_rev_i_list N)(*;*) 621 : 622 : fun reduce f u nil = u 623 : | reduce f u (x::xs) = f x (reduce f u xs)(*;*) 624 : 625 : exception bad_reduce1(*;*) 626 : fun reduce1 f nil = raise bad_reduce1 627 : | reduce1 f (x::nil) = x 628 : | reduce1 f (x::xs) = f x (reduce1 f xs)(*;*) 629 : 630 : (* integer insert into sorted list *) 631 : fun insert (a:int) nil = [a] 632 : | insert a (b::rest) = 633 : if (a < b) then (a::b::rest) 634 : else (b::(insert a rest))(*;*) 635 : 636 : (* integer sort routine *) 637 : fun int_sort nil = nil 638 : | int_sort ((a:int)::rest) = insert a (int_sort rest) 639 : 640 : (*=================================================================*) 641 : (* BOOLEAN *) 642 : (* ======= *) 643 : 644 : fun xor (x,y) = (x andalso (not y)) orelse (y andalso (not x))(*;*) 645 : 646 : (*=================================================================*) 647 : (* GEOMETRIC *) 648 : (* ========= *) 649 : 650 : (*=================================================================*) 651 : (* VECTOR *) 652 : (* ====== *) 653 : 654 : (* VECTOR TYPE *) 655 : (* ABSTRACT TYPE DEFINITION 656 : access functions 657 : vector ( make vector ) 658 : unvector V_tuple ( unmake a vector ) 659 : V_add V_sub V_equal ( add, subtract test equal two vectors ) 660 : V_smult V_sdiv ( scalar multiply vector ) 661 : V_dot V_norm ( dot product, vector norm ) 662 : V_dist V_unit ( distance between two vectors, unit vector ) 663 : V_cross V_orto ( cross product, orthonormal ) 664 : V_x V_y V_z ( get x,y,z values of vector ) 665 : values 666 : V_zero (the origin) 667 : *) 668 : abstype Vector = Vector of {x:real,y:real,z:real} 669 : with 670 : exception div_by_zero 671 : fun vector (ix,iy,iz) = Vector {x=ix,y=iy,z=iz} 672 : fun unvector (Vector({x,y,z})) = (x,y,z) 673 : fun V_add (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 674 : vector(x1+x2,y1+y2,z1+z2) 675 : fun V_sub (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 676 : vector(x1-x2,y1-y2,z1-z2) 677 : fun V_smult (N:real) (Vector({x,y,z})) = 678 : vector(N*x,N*y,N*z) 679 : fun V_sdiv (Vector({x,y,z})) (N:real)= 680 : if real_zero N then raise div_by_zero 681 : else 682 : vector(x/N,y/N,z/N) 683 : fun V_dot (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 684 : ( (x1*x2)+(y1*y2)+(z1*z2) ) 685 : fun V_norm (v:Vector) = 686 : sqrt(V_dot v v) 687 : fun V_dist Vec1 Vec2 = 688 : ( V_norm ( V_sub Vec1 Vec2 ) ) 689 : fun V_unit (V:Vector) = 690 : let val vnorm = V_norm V 691 : in 692 : if real_zero vnorm 693 : then 694 : vector(0.0,0.0,0.0) 695 : else 696 : V_sdiv V (V_norm V) 697 : end 698 : fun V_equal (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 699 : real_equal x1 x2 andalso 700 : real_equal y1 y2 andalso 701 : real_equal z1 z2 702 : fun V_cross (Vector({x=ax,y=ay,z=az})) (Vector({x=bx,y=by,z=bz})) = 703 : vector( (ay*bz) - (az*by), (az*bx) - (ax*bz), (ax*by)-(ay*bx)) 704 : fun V_orto (V1:Vector) (V2:Vector) = 705 : real_zero (V_dot V1 V2) 706 : fun V_translation (V1:Vector) (V2:Vector) = 707 : V_sub V1 V2 708 : fun V_x (Vector({x,...})) = x 709 : fun V_y (Vector({y,...})) = y 710 : fun V_z (Vector({z,...})) = z 711 : fun V_tuple (Vector({x,y,z})) = (x,y,z) 712 : fun show_vector (Vector({x,y,z})) (file:outstream) = 713 : ( putreal file x 5; 714 : putstring file " "; 715 : putreal file y 5; 716 : putstring file " "; 717 : putreal file z 5; 718 : putstring file " " 719 : ) 720 : fun V_zero () = vector(0.0,0.0,0.0) 721 : end(*;*) 722 : 723 : (* AUXILLIARY VECTOR FUNCTIONS *) 724 : 725 : (* Take vector V and return V1 a vector in the direction V, with 726 : length R *) 727 : fun move_point (R:real) (V:Vector) = V_smult R (V_unit V)(*;*) 728 : 729 : (*=================================================================*) 730 : 731 : 732 : (* val _ = use "First"; *) 733 : 734 : 735 : (* constants for camera values *) 736 : 737 : val XOFFSET = 128.0(*;*) 738 : val YOFFSET = 128.0(*;*) 739 : val SCALEFACTOR = 0.020312(*;*) 740 : val FOCALLENGTH = 16.0(*;*) 741 : 742 : (* metric baselines *) 743 : 744 : val THRESHOLD = 10.0 * SCALEFACTOR(*;*) (*10 pixels in mm *) 745 : val MAX_BOUNDING_BOX = 500.0 * SCALEFACTOR(*;*) 746 : val MIN_BOUNDING_BOX = 20.0 * SCALEFACTOR(*;*) 747 : 748 : (* other useful constants *) 749 : 750 : val M_file_extension = ".nff"(*;*) 751 : val S_junc_extension = ".jnc"(*;*) 752 : val S_arcs_extension = ".arc"(*;*) 753 : 754 : (* simple 2d to 3d and 3d to 2d transformations *) 755 : 756 : (* CAMERA_ADJUST *) 757 : (* change camera units to mm and move origin of coord system *) 758 : fun camera_adjust (x:real,y:real) = 759 : ((x-XOFFSET)*SCALEFACTOR,(y-YOFFSET)*SCALEFACTOR,FOCALLENGTH)(*;*) 760 : (* ((x*SCALEFACTOR),(y*SCALEFACTOR),FOCALLENGTH); *) 761 : 762 : (* PROJECTION *) 763 : (* onto camera_adjusted coord system above at Focal length from camera *) 764 : fun projection (x:real,y:real,z:real) = 765 : let val xn = x*(FOCALLENGTH/z) 766 : val yn = y*(FOCALLENGTH/z) 767 : in 768 : (xn,yn,FOCALLENGTH) 769 : end(*;*) 770 : 771 : 772 : 773 : 774 : (* val _ = use "Types"; *) 775 : type PTS_2 = real*real(*;*) 776 : type PTS_3 = real*real*real(*;*) 777 : 778 : (* VECTOR TYPE *) 779 : 780 : abstype Vector = Vector of {x:real,y:real,z:real} 781 : with 782 : exception div_by_zero 783 : fun vector (ix,iy,iz) = Vector {x=ix,y=iy,z=iz} 784 : fun V_add (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 785 : vector(x1+x2,y1+y2,z1+z2) 786 : fun V_sub (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 787 : vector(x1-x2,y1-y2,z1-z2) 788 : fun V_smult (N:real) (Vector({x,y,z})) = 789 : vector(N*x,N*y,N*z) 790 : fun V_sdiv (Vector({x,y,z})) (N:real)= 791 : if real_zero N then raise div_by_zero 792 : else 793 : vector(x/N,y/N,z/N) 794 : fun V_dot (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 795 : ( (x1*x2)+(y1*y2)+(z1*z2) ) 796 : fun V_norm (v:Vector) = 797 : sqrt(V_dot v v) 798 : fun V_dist Vec1 Vec2 = 799 : ( V_norm ( V_sub Vec1 Vec2 ) ) 800 : fun V_unit (V:Vector) = 801 : let val vnorm = V_norm V 802 : in 803 : if real_zero vnorm 804 : then 805 : vector(0.0,0.0,0.0) 806 : else 807 : V_sdiv V (V_norm V) 808 : end 809 : fun V_equal (Vector({x=x1,y=y1,z=z1})) (Vector({x=x2,y=y2,z=z2})) = 810 : real_equal x1 x2 andalso 811 : real_equal y1 y2 andalso 812 : real_equal z1 z2 813 : fun V_cross (Vector({x=ax,y=ay,z=az})) (Vector({x=bx,y=by,z=bz})) = 814 : vector( (ay*bz) - (az*by), (az*bx) - (ax*bz), (ax*by)-(ay*bx)) 815 : fun V_orto (V1:Vector) (V2:Vector) = 816 : real_zero (V_dot V1 V2) 817 : fun V_translation (V1:Vector) (V2:Vector) = 818 : V_sub V1 V2 819 : fun V_x (Vector({x,...})) = x 820 : fun V_y (Vector({y,...})) = y 821 : fun V_z (Vector({z,...})) = z 822 : fun V_tuple (Vector({x,y,z})) = (x,y,z) 823 : fun show_vector(Vector({x,y,z})) = 824 : ( putreal FILEOUT x 5; 825 : putstring FILEOUT " "; 826 : putreal FILEOUT y 5; 827 : putstring FILEOUT " "; 828 : putreal FILEOUT z 5; 829 : putstring FILEOUT " " 830 : ) 831 : fun V_zero () = vector(0.0,0.0,0.0) 832 : end(*;*) 833 : 834 : 835 : 836 : (* TRIPLE TYPE *) 837 : 838 : abstype Triple = Triple of Vector*Vector*Vector 839 : with 840 : fun triple(V1,V2,V3) = Triple(V1,V2,V3) 841 : fun T_u (Triple(u,_,_)) = u 842 : fun T_v (Triple(_,v,_)) = v 843 : fun T_w (Triple(_,_,w)) = w 844 : fun T_orthonormal (T:Triple) = 845 : let val u_v_diff = V_sub (T_u T) (T_v T) 846 : val a = V_sdiv (u_v_diff) (V_norm u_v_diff) 847 : val u_w_diff = V_sub (T_u T) (T_w T) 848 : val btop = V_sub (u_w_diff) (V_smult (V_dot (u_w_diff) a) a) 849 : val b = V_sdiv btop (V_norm btop) 850 : in 851 : triple ( a,b, V_cross a b) 852 : end 853 : fun T_calc_atof (Triple(u,v,w)) (Triple(upp,vpp,wpp)) = 854 : (* generate the abcdef values for the quartic solver *) 855 : ( sqr( (V_norm (V_sub u v))), 856 : sqr( (V_norm (V_sub u w))), 857 : sqr( (V_norm (V_sub v w))), 858 : V_dot (V_unit upp) (V_unit vpp), 859 : V_dot (V_unit upp) (V_unit wpp), 860 : V_dot (V_unit vpp) (V_unit wpp) 861 : ) 862 : fun T_calc_single_primes (Triple(u,v,w)) (l,m,n) = 863 : triple (V_smult l (V_unit u), V_smult m (V_unit v), V_smult n (V_unit w) ) 864 : fun show_triple(Triple(u,v,w)) = 865 : (show_vector(u); 866 : putstring FILEOUT "\n"; 867 : show_vector(v); 868 : putstring FILEOUT "\n"; 869 : show_vector(w); 870 : putstring FILEOUT "\n") 871 : end(*;*) 872 : 873 : 874 : 875 : 876 : (* MATRIX TYPE *) 877 : 878 : 879 : abstype Matrix = Matrix of {a11:real,a12:real,a13:real,a21:real,a22:real,a23:real,a31:real,a32:real,a33:real} 880 : with 881 : fun Matrix_zero () = 882 : Matrix {a11=0.0,a12=0.0,a13=0.0, 883 : a21=0.0,a22=0.0,a23=0.0, 884 : a31=0.0,a32=0.0,a33=0.0} 885 : 886 : fun matrix (T1:Triple) (T2:Triple) = 887 : let val Vu = T_u T1 888 : val Vv = T_v T1 889 : val Vw = T_w T1 890 : val Vup = T_u T2 891 : val Vvp = T_v T2 892 : val Vwp = T_w T2 893 : in 894 : Matrix 895 : { a11 = ((V_x Vu)*(V_x Vup))+((V_x Vv)*(V_x Vvp))+((V_x Vw)*(V_x Vwp)), 896 : a12 = ((V_x Vu)*(V_y Vup))+((V_x Vv)*(V_y Vvp))+((V_x Vw)*(V_y Vwp)), 897 : a13 = ((V_x Vu)*(V_z Vup))+((V_x Vv)*(V_z Vvp))+((V_x Vw)*(V_z Vwp)), 898 : a21 = ((V_y Vu)*(V_x Vup))+((V_y Vv)*(V_x Vvp))+((V_y Vw)*(V_x Vwp)), 899 : a22 = ((V_y Vu)*(V_y Vup))+((V_y Vv)*(V_y Vvp))+((V_y Vw)*(V_y Vwp)), 900 : a23 = ((V_y Vu)*(V_z Vup))+((V_y Vv)*(V_z Vvp))+((V_y Vw)*(V_z Vwp)), 901 : a31 = ((V_z Vu)*(V_x Vup))+((V_z Vv)*(V_x Vvp))+((V_z Vw)*(V_x Vwp)), 902 : a32 = ((V_z Vu)*(V_y Vup))+((V_z Vv)*(V_y Vvp))+((V_z Vw)*(V_y Vwp)), 903 : a33 = ((V_z Vu)*(V_z Vup))+((V_z Vv)*(V_z Vvp))+((V_z Vw)*(V_z Vwp)) 904 : } 905 : end 906 : fun calc_angles (Matrix({a11,a12,a13,a21,a22,a23,a31,a32,a33})) = 907 : if (real_zero a31) andalso (real_zero a32 ) 908 : then (0.0, atan2 (a31,a33), atan2 (a12,a11) ) 909 : else 910 : let val Phi = atan2( a31,~a32) 911 : val Theta = atan2 ( (a31*(sin Phi)-(a32*(cos Phi))),a33) 912 : val Psi = atan2(a13,a23) 913 : in 914 : (Psi,Theta,Phi) 915 : end 916 : fun apply_R_to_V (Matrix({a11,a12,a13,a21,a22,a23,a31,a32,a33})) (V:Vector) = 917 : let val x = V_x V 918 : val y = V_y V 919 : val z = V_z V 920 : in 921 : vector (a11*x+a21*y+a31*z, 922 : a12*x+a22*y+a32*z, 923 : a13*x+a23*y+a33*z ) 924 : end 925 : fun apply_R (M:Matrix) (T:Triple) = 926 : triple((apply_R_to_V M (T_u T)),(apply_R_to_V M (T_v T)),(apply_R_to_V M (T_w T)) ) 927 : fun print_matrix (FILE:outstream) (Matrix({a11,a12,a13,a21,a22,a23,a31,a32,a33})) = 928 : (putstring FILE "\na11 "; 929 : putreal FILE a11 5; 930 : putstring FILE "\ta12 "; 931 : putreal FILE a12 5; 932 : putstring FILE "\ta13 "; 933 : putreal FILE a13 5; 934 : putstring FILE "\na21 "; 935 : putreal FILE a21 5; 936 : putstring FILE "\ta22 "; 937 : putreal FILE a22 5; 938 : putstring FILE "\ta23 "; 939 : putreal FILE a23 5; 940 : putstring FILE "\na31 "; 941 : putreal FILE a31 5; 942 : putstring FILE "\ta32 "; 943 : putreal FILE a32 5; 944 : putstring FILE "\ta33 "; 945 : putreal FILE a33 5; 946 : putstring FILE "\n" ) 947 : 948 : end(*;*) 949 : 950 : 951 : 952 : 953 : (* POINTS TYPE *) 954 : (* id number, vector, type *) 955 : (* type is one of junction,free,centre *) 956 : 957 : 958 : datatype Point_type = junct | free | centre (*;*) 959 : datatype Point = point of int * Vector * Point_type (*;*) 960 : 961 : 962 : 963 : abstype point_set = point_set of Point list 964 : with 965 : 966 : fun POINT_KILL (point_set ps) = ps 967 : 968 : 969 : 970 : fun size_point ( point_set ps) = 971 : length ps(*;*) 972 : 973 : 974 : val Point_empty = point_set nil 975 : exception not_point 976 : 977 : fun is_point' (Pnum:int) nil = false 978 : | is_point' (Pnum:int) (point(Inum,_,_)::Rest) = 979 : if Pnum = Inum then true 980 : else is_point' Pnum Rest 981 : 982 : fun is_point (Pnum:int) (point_set PS) = 983 : is_point' Pnum PS 984 : 985 : local 986 : fun point_member (point(Pnum,_,_)) (ps: Point list) = 987 : (is_point' Pnum ps) 988 : in 989 : fun Point_insert (p:Point) (point_set ps) = 990 : if (point_member p ps) 991 : then (point_set ps) 992 : else (point_set (p::ps)) 993 : end 994 : 995 : local 996 : fun retrieve_point _ nil = raise not_point 997 : | retrieve_point pnum (point(Inum,Vec,Ptype) :: Rest) = 998 : if pnum = Inum 999 : then (Inum,Vec,Ptype) 1000 : else retrieve_point pnum Rest 1001 : in 1002 : fun Point_get (pnum:int) (point_set ps) = 1003 : retrieve_point pnum ps 1004 : end 1005 : 1006 : local 1007 : fun Point_find' V nil = 0 1008 : | Point_find' V (point(Num,PVec,Ptype) :: Rest) = 1009 : if V_equal V PVec 1010 : then Num 1011 : else Point_find' V Rest 1012 : in 1013 : fun Point_find (V:Vector) (point_set ps) = 1014 : Point_find' V ps 1015 : end 1016 : 1017 : local 1018 : fun remove_point _ nil = raise not_point 1019 : | remove_point p ( point(Inum,Vec,Ptype) ::Rest) = 1020 : if p = Inum 1021 : then Rest 1022 : else ( point(Inum,Vec,Ptype) :: (remove_point p Rest) ) 1023 : in 1024 : fun Point_remove (p:int) (point_set ps) = 1025 : point_set (remove_point p ps) 1026 : end 1027 : 1028 : local 1029 : fun point_projection (point(Inum,Vec,Ptype)) = 1030 : let val VecO = vector(projection (V_tuple Vec)) 1031 : in 1032 : point(Inum,VecO,Ptype) 1033 : end 1034 : 1035 : 1036 : fun apply_M_T M T V:Vector = 1037 : V_add T (apply_R_to_V M V) 1038 : 1039 : fun point_transform (Rot,Trans) (point(Inum,Vec,Ptype)) = 1040 : point(Inum,(apply_M_T Rot Trans Vec),Ptype) 1041 : 1042 : fun old_point_transform ps M_T = 1043 : map (point_projection o (point_transform M_T) ) ps 1044 : 1045 : fun XY_compare Newv Large Small = 1046 : (max(Newv, Large), min(Newv,Small) ) 1047 : 1048 : fun get_bound_box' nil MaxX MinX MaxY MinY = (MaxX, MinX, MaxY, MinY) 1049 : | get_bound_box' ( point(_,Vec1,_)::Rest ) MaxX MinX MaxY MinY = 1050 : let val Vx = V_x Vec1 1051 : val Vy = V_y Vec1 1052 : val (Newxmax,Newxmin) = XY_compare Vx MaxX MinX 1053 : val (Newymax,Newymin) = XY_compare Vy MaxY MinY 1054 : in 1055 : get_bound_box' Rest Newxmax Newxmin Newymax Newymin 1056 : end 1057 : 1058 : fun get_bound_box point_list = 1059 : get_bound_box' point_list 0.0 9999.0 0.0 9999.0 1060 : in 1061 : fun Point_transform (point_set ps) M_T = 1062 : let val New_point_set = old_point_transform ps M_T 1063 : val (MaxX,MinX,MaxY,MinY) = get_bound_box New_point_set 1064 : val bound_size = sqrt ( sqr(MaxX-MinX) + sqr(MaxY-MinY) ) 1065 : in 1066 : (point_set New_point_set,bound_size) 1067 : end 1068 : end 1069 : 1070 : 1071 : fun Point_vector p ps = 1072 : let val (Num,Vec,Ptype) = Point_get p ps 1073 : in 1074 : Vec 1075 : end 1076 : 1077 : fun Point_type p ps = 1078 : let val (Num,Vec,Ptype) = Point_get p ps 1079 : in 1080 : Ptype 1081 : end 1082 : 1083 : fun Point_len (point_set ps) = 1084 : len ps 1085 : 1086 : end(*;*) 1087 : 1088 : (* val _ = use "Complex"; *) 1089 : 1090 : (* complex number routines *) 1091 : 1092 : abstype complex = complex of real*real 1093 : with 1094 : val C_zero = complex(0.0,0.0) 1095 : fun Complex(x,y) = complex(x,y) 1096 : fun C_real (complex(r,_)) = r 1097 : fun C_imag (complex(_,i)) = i 1098 : fun C_add (complex(ar,ai)) (complex(br,bi)) = 1099 : Complex( (ar+br),(ai+bi)) 1100 : fun C_mult(complex(ar,ai)) (complex(br,bi)) = 1101 : Complex( ((ar*br) -(ai*bi)) , ( (ar*bi) + (ai*br))) 1102 : fun C_rmult(r:real) (complex(ar,ai)) = 1103 : Complex(r*ar,r*ai) 1104 : fun C_abs (complex(r,i)) = 1105 : sqrt( (sqr r) + (sqr i) ) 1106 : fun C_pow C 0 = Complex(1.0,0.0) 1107 : | C_pow C N = C_mult C (C_pow C (N-1)) 1108 : fun C_powr (complex(ar,ai)) (x:real) = 1109 : if (real_zero ai) 1110 : then 1111 : if (ar>0.0) then 1112 : Complex( (pow ar x),0.0) 1113 : else 1114 : Complex( 0.0,(pow (~ar) x)) 1115 : else 1116 : let val m=C_abs(Complex(ar,ai)) 1117 : val a = atan2(ai,ar) 1118 : val ms = pow m x 1119 : val ax = a*x 1120 : in 1121 : Complex((ms*(cos ax)),(ms*(sin ax))) 1122 : end 1123 : 1124 : end(*;*) 1125 : 1126 : 1127 : fun print_complex (C:complex) = 1128 : ( print "complex( "; 1129 : print (C_real C); 1130 : print " , "; 1131 : print (C_imag C); 1132 : print " )\n"; 1133 : () 1134 : )(*;*) 1135 : 1136 : (* val _ = use "Quartic"; *) 1137 : (* solve quartic equations *) 1138 : 1139 : 1140 : 1141 : 1142 : 1143 : 1144 : 1145 : 1146 : fun real_coeffs (a3,a2,a1,a0) (u:real) = 1147 : ( ( ((sqr a3)/4.0) + u-a2) >= 0.0 ) andalso 1148 : ( ((sqr (u/2.0)) - a0 ) >= 0.0 ) (*;*) 1149 : 1150 : 1151 : local fun t1_calc arg2 arg3 argu= 1152 : let val ts =( ( (sqr arg3) /4.0 ) +argu -arg2) 1153 : in 1154 : if ts >= 0.0 1155 : then 1156 : (let val t1 = (sqrt ts) 1157 : val out3= Complex( (arg3/2.0)+t1 , 0.0) 1158 : val out1= Complex( (arg3/2.0)-t1 , 0.0) 1159 : in 1160 : (out1,out3) 1161 : end 1162 : ) 1163 : else 1164 : (let val t1 = (sqrt (~ts)); 1165 : val out3= Complex( (arg3/2.0) , t1) 1166 : val out1= Complex( (arg3/2.0) , (~t1)) 1167 : in 1168 : (out1,out3) 1169 : end 1170 : ) 1171 : end 1172 : fun t2_calc arg0 arg1 argu = 1173 : let val ts = ( (sqr (argu/2.0)) - arg0 ) 1174 : in 1175 : if ts >= 0.0 1176 : then 1177 : (let val t2= (sqrt ts) 1178 : val out2 = Complex( (argu/2.0)-t2 , 0.0) 1179 : val out0 = Complex( (argu/2.0)+t2 , 0.0) 1180 : in 1181 : (out0,out2) 1182 : end 1183 : ) 1184 : else 1185 : (let val t2= (sqrt (~ts)) 1186 : val out2 = Complex (argu/2.0,(~t2)) 1187 : val out0 = Complex (argu/2.0,t2) 1188 : in 1189 : (out0,out2) 1190 : end 1191 : ) 1192 : end 1193 : in fun calc_quad_coeffs (a3,a2,a1,a0) u = 1194 : let val (d1,d3) = t1_calc a2 a3 u 1195 : val (d0,d2) = t2_calc a0 a2 u 1196 : in 1197 : (d0,d1,d2,d3) 1198 : end 1199 : end(*;*) 1200 : 1201 : 1202 : 1203 : fun one_quad_solve (d0,d1) = 1204 : let val q = C_add (C_pow d1 2) (C_rmult (~4.0) d0) 1205 : in 1206 : if real_zero (C_imag q) 1207 : then 1208 : if (C_real q) < 0.0 1209 : then 1210 : ( let val t = sqrt(~(C_real q)) 1211 : val s0 = Complex( ~(C_real d1)/2.0, (t/2.0)) 1212 : val s1 = Complex( ~(C_real d1)/2.0, (~t/2.0)) 1213 : in 1214 : (s0,s1) 1215 : end 1216 : ) 1217 : else 1218 : ( let val t = sqrt(C_real q) 1219 : val s0 = Complex( (~(C_real d1)/2.0) +(t/2.0), 0.0 ) 1220 : val s1 = Complex( (~(C_real d1)/2.0) -(t/2.0), 0.0 ) 1221 : in 1222 : (s0,s1) 1223 : end 1224 : ) 1225 : else 1226 : ( 1227 : let val ct = C_powr q 0.5 1228 : val s0 = Complex ( (~(C_real d1)/2.0) + (C_real ct)/2.0 , 1229 : (~(C_imag d1)/2.0) + (C_imag ct)/2.0 ) 1230 : val s1 = Complex ( (~(C_real d1)/2.0) - (C_real ct)/2.0 , 1231 : (~(C_imag d1)/2.0) - (C_imag ct)/2.0 ) 1232 : in 1233 : (s0,s1) 1234 : end 1235 : ) 1236 : end(*;*) 1237 : 1238 : fun quad_solve (d0,d1,d2,d3) = 1239 : let val (s0,s1) = one_quad_solve(d0,d1) 1240 : val (s2,s3) = one_quad_solve(d2,d3) 1241 : in 1242 : (s0,s1,s2,s3) 1243 : end(*;*) 1244 : 1245 : 1246 : fun cubic_solve (a3,a2,a1,a0) (c0,c1,c2) = 1247 : let 1248 : val t = (c1/3.0) - ( (sqr c2)/9.0 ) 1249 : val r = ( (((c1*c2) - (3.0*c0) )/6.0) - ((cube c2)/27.0)) 1250 : val d = (cube t) + (sqr r) 1251 : in 1252 : if (d>0.0) 1253 : then (true, ( (cube_root (r+(sqrt d)) ) + 1254 : (cube_root (r-(sqrt d)) )- 1255 : (c2/3.0) ) 1256 : ) 1257 : else 1258 : let val m = pow ( (~d) + (sqr r)) (1.0/6.0) 1259 : val a = atan2( (sqrt (~d)), r) 1260 : val sps = m*2.0* (cos (a/3.0)) 1261 : val sms = m*2.0* (sin (a/3.0)) 1262 : val u0 = sps - (c2/3.0) 1263 : val u1 = ((~sps)/2.0) - (c2/3.0) - (sms*(sqrt 3.0)/2.0) 1264 : val u2 =( (~sps)/2.0) - (c2/3.0) + (sms*(sqrt 3.0)/2.0) 1265 : in 1266 : if real_coeffs (a3,a2,a1,a0) u0 then (true,u0) 1267 : else if real_coeffs (a3,a2,a1,a0) u1 then (true,u1) 1268 : else if real_coeffs (a3,a2,a1,a0) u2 then (true,u2) 1269 : else (false,0.0) 1270 : end 1271 : end(*;*) 1272 : 1273 : fun calc_cubic_coeffs (a3,a2,a1,a0) = 1274 : ( ~( (sqr a1) + (a0 * (sqr a3)) - (4.0*a0*a2) ), 1275 : ( (a1*a3) - (4.0*a0)), 1276 : (~a2) 1277 : ) (*;*) 1278 : 1279 : fun quartic_solve (a:real*real*real*real) = 1280 : let val cubic_coeffs = calc_cubic_coeffs a 1281 : val (Success,U1_real_root) = cubic_solve a cubic_coeffs 1282 : in 1283 : if Success 1284 : then 1285 : (Success, (quad_solve ( calc_quad_coeffs a U1_real_root ))) 1286 : else 1287 : (Success,(C_zero,C_zero,C_zero,C_zero)) 1288 : end(*;*) 1289 : fun quart (a,b,c,d:real) x = 1290 : sqr( sqr(x)) + (a * (cube x) ) + (b * (sqr x)) + (c*x) + d(*;*) 1291 : 1292 : fun strip_poor_answers [] _ = [] 1293 : | strip_poor_answers (x::xs) a = 1294 : if abs( quart a x) >1E~5 then (strip_poor_answers xs a) 1295 : else (x::( strip_poor_answers xs a) )(*;*) 1296 : 1297 : fun strip_none_reals [] = [] 1298 : | strip_none_reals (x::xs) = 1299 : if real_zero (C_imag x) 1300 : then ( (C_real x):: (strip_none_reals xs)) 1301 : else (strip_none_reals xs)(*;*) 1302 : 1303 : fun quartic (a: real*real*real*real) = 1304 : let val (Success,(r0,r1,r2,r3)) = quartic_solve a 1305 : val real_results = nil 1306 : in 1307 : if Success 1308 : then 1309 : strip_poor_answers (strip_none_reals [r0,r1,r2,r3]) a 1310 : else 1311 : (* no_results_from_quartic *) 1312 : [] 1313 : end(*;*) 1314 : 1315 : 1316 : 1317 : fun quartic_test (q: real*real*real*real) = 1318 : let val (Succ,(R0,R1,R2,R3)) = quartic_solve q 1319 : in 1320 : if Succ 1321 : then 1322 : ( print_complex R0; 1323 : print_complex R1; 1324 : print_complex R2; 1325 : print_complex R3 1326 : ) 1327 : else (print "Better luck next time!";()) 1328 : end(*;*) 1329 : 1330 : 1331 : 1332 : 1333 : (* val _ = use "Input_modules"; *) 1334 : (* handle the input of model and scene information *) 1335 : (* convert to vectors and triples and other *) 1336 : (* abstypes as soon as feasible. *) 1337 : 1338 : (* exported functions expected to be *) 1339 : (* read_model *) 1340 : (* read_scene *) 1341 : 1342 : 1343 : 1344 : (* some datatypes *) 1345 : datatype COMPONENTS = polygon of (real*real*real) list 1346 : | cylinder of ((real*real*real*real)*(real*real*real*real))(*;*) 1347 : 1348 : datatype RAW_OBJECTS = junc of (int* (real*real*real) *(int list)) 1349 : | arc of (int*(real*real*real)*real)(*;*) 1350 : 1351 : datatype OBJECTS = junction of (int*(int list)) 1352 : | loose of (int*int) 1353 : | foci of (int * real)(*;*) 1354 : 1355 : 1356 : 1357 : (* some useful simple functions *) 1358 : 1359 : (* original code ... 1360 : 1361 : fun get_file_model () = (* get_filename of model *) 1362 : ( output(std_out,"\nFile containing model :\n"); 1363 : read_to_eol std_in )(*;*) 1364 : 1365 : 1366 : fun get_file_scene ()= (* get scene name *) 1367 : let val fname = ( output(std_out,"\nFile containing scene :\n"); 1368 : skip std_in; 1369 : read_to_eol std_in ) 1370 : val YN = ( output(std_out,"\nArcs file (y/n)? :\n"); 1371 : skip std_in; 1372 : input(std_in,1) 1373 : ) 1374 : in 1375 : (fname,YN) 1376 : end(*;*) 1377 : *) 1378 : 1379 : fun get_file_model () = "programs/DATA/bsim" 1380 : 1381 : fun get_file_scene ()= (* get scene name *) 1382 : let val fname = "programs/DATA/bsim" 1383 : val YN = "y" 1384 : in 1385 : (fname,YN) 1386 : end(*;*) 1387 : 1388 : 1389 : 1390 : exception early_eof(*;*) 1391 : fun safe_get_int (s:instream) = 1392 : if end_of_stream s then raise early_eof 1393 : else getint s(*;*) 1394 : fun safe_get_real (s:instream) = 1395 : real(safe_get_int s)(*;*) 1396 : 1397 : 1398 : 1399 : 1400 : 1401 : 1402 : (*==================================================================*) 1403 : 1404 : 1405 : 1406 : 1407 : (* READ MODEL *) 1408 : (* read_model takes model data forms objects (polygons, cylinders) *) 1409 : (* and produces an indexed points list *) 1410 : (* most of the objects will be based around the indices *) 1411 : (* of the points to reduce repetition. *) 1412 : (* model input *) 1413 : 1414 : 1415 : local 1416 : exception e_read_polygon(*;*) 1417 : exception e_read_cylinder(*;*) 1418 : fun read_polygon_lines (s:instream) 0 = nil 1419 : | read_polygon_lines (s:instream) n = 1420 : let val x =getsimreal s 1421 : val y =getsimreal s 1422 : val z =getsimreal s 1423 : in 1424 : (x,y,z) :: (read_polygon_lines s (n-1)) 1425 : end 1426 : 1427 : fun read_polygon (s:instream) = 1428 : (if lookahead s = "p" then (input(s,1)) 1429 : else raise e_read_polygon; 1430 : polygon( (read_polygon_lines s (safe_get_int s)) ) 1431 : ) 1432 : 1433 : fun read_cylinder_line (s:instream) = 1434 : let val x= getsimreal s; 1435 : val y= getsimreal s; 1436 : val z= getsimreal s; 1437 : val r= getsimreal s; 1438 : in 1439 : (x,y,z,r) 1440 : end 1441 : fun read_cylinder (s:instream) = 1442 : ( 1443 : if lookahead s = "c" then skip_to_eol s 1444 : else raise e_read_cylinder; 1445 : cylinder( (read_cylinder_line s , read_cylinder_line s) ) 1446 : ) 1447 : fun read_components (s:instream) = 1448 : (skip s; 1449 : case (lookahead s) 1450 : of "p" => (read_polygon s)::(read_components s) 1451 : | "c" => (read_cylinder s)::(read_components s) 1452 : | "" => nil 1453 : | _ => (skip_to_eol s;read_components s) 1454 : ) 1455 : in 1456 : fun read_model () = 1457 : let val s = open_in ((get_file_model ())^M_file_extension) 1458 : in 1459 : read_components s 1460 : end 1461 : 1462 : end(*;*) 1463 : 1464 : 1465 : 1466 : (*==================================================================*) 1467 : 1468 : 1469 : 1470 : (* PROCESS MODEL *) 1471 : (* needs a points list, as well as to make L shapes from the *) 1472 : (* model components :may delay L shapes as well get loads! *) 1473 : 1474 : (* PROCESSING POLYGONS is pretty tricky, there are two levels *) 1475 : (* of junction creation going on. One is at the EACH polygon *) 1476 : (* level, the other is at the ALL POLYGONS level. *) 1477 : (* hence the function add_polygon returns the first level objects. *) 1478 : (* an object list at the each polygon level. Then Tidy_Objs *) 1479 : (* makes the ALL polygons level process and differentiates between *) 1480 : (* juncts and loose lines *) 1481 : local 1482 : 1483 : fun include_junc Id Id_list nil = [junction(Id,Id_list)] 1484 : | include_junc Id Id_list (junction(Idj,Idj_list)::Rest) = 1485 : if Idj = Id 1486 : then 1487 : (junction(Id, merge (Id_list,Idj_list) ) :: Rest) 1488 : else 1489 : (junction(Idj,Idj_list) :: (include_junc Id Id_list Rest)) 1490 : | include_junc Id Id_list New_list = 1491 : (hd New_list) :: (include_junc Id Id_list (tl New_list) ) 1492 : 1493 : 1494 : fun Tidy_objs' nil New_list = New_list 1495 : | Tidy_objs' (foci(Id,Rad)::Rest) New_list = 1496 : Tidy_objs' Rest (foci(Id,Rad)::New_list) 1497 : | Tidy_objs' (loose(Id,Id2)::Rest) New_list = 1498 : Tidy_objs' Rest (loose(Id,Id2)::New_list) 1499 : | Tidy_objs' (junction(Id,Id_list) :: Rest) New_list = 1500 : Tidy_objs' Rest (include_junc Id Id_list New_list) 1501 : 1502 : 1503 : fun Tidy_objs Obs_list = 1504 : Tidy_objs' Obs_list nil 1505 : 1506 : fun get_PID V PS Pnum = 1507 : let val PID = Point_find V PS 1508 : in 1509 : if PID = 0 1510 : then (* new point*) 1511 : (Pnum+1, Point_insert (point(Pnum+1,V,junct)) PS,Pnum+1) 1512 : else (* point exists *) 1513 : (PID,PS,Pnum) 1514 : end 1515 : 1516 : fun change_to_PID_list nil PS Pnum = (nil,PS,Pnum) 1517 : | change_to_PID_list ( (XYZ)::Rest ) PS Pnum = 1518 : let val (PID,PSnew,Pnumnew) = get_PID (vector(XYZ)) PS Pnum 1519 : val (R_PIDs,FinalPS,FinalPnum) = change_to_PID_list Rest PSnew Pnumnew 1520 : in 1521 : (PID::R_PIDs,FinalPS,FinalPnum) 1522 : end 1523 : 1524 : exception junc_pid_problem 1525 : 1526 : fun gen_junc_list' [A,B,C] = [junction(B,[A,C])] 1527 : | gen_junc_list' P_list = 1528 : (junction(hd (tl P_list),[hd(P_list),hd(tl(tl P_list))]) :: (gen_junc_list' (tl P_list) ) ) 1529 : 1530 : fun gen_junc_list nil = raise junc_pid_problem 1531 : | gen_junc_list [A] = raise junc_pid_problem 1532 : | gen_junc_list [A,B] = raise junc_pid_problem 1533 : | gen_junc_list PIDs = 1534 : gen_junc_list' ( PIDs @ [ hd PIDs, (hd (tl PIDs)) ] ) 1535 : (* last step makes the polygon closed, ie not 1234, but 123412, which *) 1536 : (* simplifies the junction list making ie [2,1,3] [3,2,4] etc. *) 1537 : 1538 : 1539 : fun add_polygon P_list PS Pnum = 1540 : let val (PID_list, NPS, NPnum) = change_to_PID_list P_list PS Pnum 1541 : val level_1_junc_list = gen_junc_list PID_list 1542 : in 1543 : (level_1_junc_list, NPS, NPnum) 1544 : end 1545 : 1546 : fun add_arc V Radius PS Pnum = 1547 : let val PID = Point_find V PS 1548 : in 1549 : if PID = 0 1550 : then 1551 : (* new point *) 1552 : (foci(Pnum+1,Radius), Point_insert (point( Pnum+1,V,centre)) PS, Pnum+1) 1553 : else 1554 : (foci(PID,Radius), PS, Pnum ) 1555 : end 1556 : 1557 : fun add_cylinder (x1,y1,z1,r1,x2,y2,z2,r2) PS Pnum = 1558 : let val (Arc1,PS1,Pnum1) = add_arc (vector(x1,y1,z1)) r1 PS Pnum 1559 : val (Arc2,PS2,Pnum2) = add_arc (vector(x2,y2,z2)) r2 PS1 Pnum1 1560 : in 1561 : ( [Arc1,Arc2], PS2, Pnum2 ) 1562 : end 1563 : 1564 : 1565 : fun process_raw_model' nil PS Last_point= (nil,PS, Last_point) 1566 : | process_raw_model' ( cylinder((x1,y1,z1,r1),(x2,y2,z2,r2)) :: Rest_C ) (PS:point_set) Last_point = 1567 : let val (Objs,PSs, Next_point) = add_cylinder (x1,y1,z1,r1,x2,y2,z2,r2) PS Last_point 1568 : val (Rest_Objs,Final_PSs,Final_point) = process_raw_model' Rest_C PSs Next_point 1569 : in 1570 : (Objs @ Rest_Objs, Final_PSs, Final_point) 1571 : end 1572 : | process_raw_model' ( polygon( Poly_list) ::Rest_C) (PS:point_set) Last_point = 1573 : let val (Objs,PSs,Next_point)= add_polygon Poly_list PS Last_point 1574 : val (Rest_Objs,Final_PSs, Final_point) = process_raw_model' Rest_C PSs Next_point 1575 : in 1576 : (Objs @ Rest_Objs, Final_PSs, Final_point) 1577 : end 1578 : 1579 : 1580 : in 1581 : fun process_raw_model (C:COMPONENTS list) = 1582 : let val (Objs,Points,Last_point_num) = process_raw_model' C Point_empty 0 1583 : in 1584 : (Tidy_objs (Objs),Points) 1585 : end 1586 : end(*;*) 1587 : 1588 : (*==================================================================*) 1589 : 1590 : (* scene data currently takes account only of the junctions *) 1591 : (* file and not of arcs etc. *) 1592 : 1593 : 1594 : 1595 : (* READ_SCENE *) 1596 : (* read_scene takes scene data, applies scaling and translations *) 1597 : (* and produces lists of junctions and points indexed *) 1598 : (* as for read_model *) 1599 : (* input model *) 1600 : local 1601 : fun get_connect_list' s 0 = nil 1602 : | get_connect_list' s n = 1603 : let val jn_num = safe_get_int s 1604 : in 1605 : ( safe_get_int s; 1606 : (jn_num) :: (get_connect_list' s (n-1)) 1607 : ) 1608 : end 1609 : 1610 : fun get_connect_list (s:instream) = 1611 : let val num= safe_get_int s 1612 : in 1613 : get_connect_list' s num 1614 : end 1615 : 1616 : fun read_junction (s:instream) = 1617 : junc( safe_get_int s, camera_adjust(getsimreal s, getsimreal s), 1618 : get_connect_list s) 1619 : 1620 : fun read_junction_list (s:instream) = 1621 : (skip s; 1622 : if end_of_stream s 1623 : then nil 1624 : else ( (read_junction s)::(read_junction_list s)) 1625 : ) 1626 : 1627 : fun read_junctions (s:instream) = 1628 : (skip_to_eol s; 1629 : skip_to_eol s; 1630 : read_junction_list s) 1631 : 1632 : fun read_arc (s:instream) (N:int) = 1633 : (safe_get_int s; 1634 : safe_get_int s; 1635 : safe_get_int s; 1636 : safe_get_int s; 1637 : safe_get_int s; 1638 : arc (N, camera_adjust(real (safe_get_int s), real (safe_get_int s)), 1639 : ( (real (safe_get_int s)) * SCALEFACTOR) ) 1640 : ) 1641 : 1642 : fun read_arc_line s N = (* to skip trailing values in arcs file *) 1643 : let val Arc = read_arc s N 1644 : in 1645 : (skip_to_eol s; 1646 : Arc) 1647 : end 1648 : 1649 : fun read_arcs_list (s:instream) (N:int) = 1650 : (skip s; 1651 : if end_of_stream s 1652 : then nil 1653 : else ( (read_arc_line s N) :: (read_arcs_list s (N+1) ) ) 1654 : ) 1655 : 1656 : 1657 : 1658 : fun read_arcs (s:instream) (N:int) = 1659 : (skip_to_eol s; 1660 : skip_to_eol s; 1661 : read_arcs_list s (N+1) ) 1662 : 1663 : in 1664 : fun read_scene () = 1665 : let val (froot,YN) = get_file_scene () 1666 : val s1 = open_in ((froot)^S_junc_extension) 1667 : val junc_list = read_junctions s1 1668 : in 1669 : if YN = "y" orelse YN ="Y" 1670 : then 1671 : let val s2 = open_in ((froot)^S_arcs_extension) 1672 : in 1673 : junc_list @ (read_arcs s2 (len junc_list) ) 1674 : end 1675 : else 1676 : junc_list 1677 : end 1678 : end(*;*) 1679 : 1680 : 1681 : 1682 : 1683 : (*==================================================================*) 1684 : 1685 : 1686 : (* PROCESS RAW SCENE *) 1687 : (* now the RAW_OBJECTS read from the scene files need to be processed *) 1688 : (* to obtain a points list, and an objects list *) 1689 : local 1690 : fun process_raw_arc (No,XYZ,Rad) = 1691 : ( foci(No,Rad) , point(No,vector XYZ,centre) ) 1692 : 1693 : fun process_raw_junction (No,XYZ,C_list) = 1694 : ( if len C_list > 1 1695 : then (* is a junction *) 1696 : ( junction(No,C_list), point(No,vector XYZ,junct) ) 1697 : else (* is a loose point *) 1698 : let val C_item = hd C_list (* really only 1 long *) 1699 : in 1700 : ( loose(No,C_item) , point(No,vector XYZ,free) ) 1701 : end 1702 : ) 1703 : 1704 : fun process_scene' nil Point_list = (nil,Point_list) 1705 : | process_scene' (junc(No,XYZ,C_list) :: Rest ) Point_list = 1706 : let val (Obs_list,New_Point_list) = process_scene' Rest Point_list 1707 : val (Obj,Point) = process_raw_junction(No,XYZ,C_list) 1708 : in 1709 : (Obj::Obs_list, Point_insert Point New_Point_list) 1710 : end 1711 : | process_scene' (arc(No,XYZ,Rad) :: Rest) Point_list = 1712 : let val (Obs_list,New_Point_list) = process_scene' Rest Point_list 1713 : val (Obj,Point) = process_raw_arc(No,XYZ,Rad) 1714 : in 1715 : (Obj::Obs_list, Point_insert Point New_Point_list) 1716 : end 1717 : 1718 : 1719 : in (* produce a tuple (Objects list, Points set) *) 1720 : fun process_raw_scene (RO:RAW_OBJECTS list) = 1721 : process_scene' RO Point_empty 1722 : end(*;*) 1723 : 1724 : 1725 : 1726 : (*======================================================================*) 1727 : 1728 : (* val _ = use "Triple_gen"; *) 1729 : (*======================================================================*) 1730 : (* Triple generation functions *) 1731 : (* some chat about why they are here *) 1732 : 1733 : (* MAKE_TRIPLES *) 1734 : (* remeber that triples are junctions that are junctions between *) 1735 : (* junctions and don't involve loose points or arc centres in *) 1736 : (* the first instance *) 1737 : 1738 : 1739 : exception bad_triple_form(*;*) 1740 : exception not_point(*;*) 1741 : 1742 : fun tidy_C nil _ = nil 1743 : | tidy_C (No::Rest) Points = 1744 : if Point_type No Points = junct 1745 : then ((Point_vector No Points) :: tidy_C Rest Points) 1746 : else tidy_C Rest Points(*;*) 1747 : 1748 : fun Form_triple [A,B,C] = 1749 : triple (B,A,C) 1750 : | Form_triple _ = raise bad_triple_form(*;*) 1751 : 1752 : fun make_triple(No:int,C_list:int list,Points:point_set) = 1753 : (* is triple if No is pivot for two junctions *) 1754 : let val tidy_C_list = tidy_C C_list Points 1755 : in 1756 : if len tidy_C_list < 2 1757 : then nil (* not enough junctions connected to point *) 1758 : else (* work out all possible Triples from the list *) 1759 : map (Form_triple o (append [ Point_vector No Points ] ) ) (permute tidy_C_list) 1760 : end(*;*) 1761 : 1762 : 1763 : fun make_triples' nil _ = nil 1764 : | make_triples' ( junction(No,C_list) :: Rest) (Points: point_set) = 1765 : if is_point No Points 1766 : then 1767 : (make_triple(No,C_list,Points) @ (make_triples' Rest Points)) 1768 : else 1769 : raise not_point 1770 : | make_triples' ( loose(No,_) :: Rest) Points = make_triples' Rest Points 1771 : | make_triples' ( foci(No,Rad) ::Rest ) Points = make_triples' Rest Points(*;*) 1772 : 1773 : 1774 : 1775 : 1776 : 1777 : (* timing switch on or off? *) 1778 : (* 1779 : val _ = if timing_wanted then ( (System.Control.Profile.profiling := true)) else (); 1780 : *) 1781 : 1782 : fun make_triples ( (OL:OBJECTS list), (Points: point_set ) ) = 1783 : ( putstring FILEOUT ("\nNumber of points ="); 1784 : putint FILEOUT (size_point Points); 1785 : putstring FILEOUT "\nNumber of junction+loose+foci objects ="; 1786 : putint FILEOUT (length OL); 1787 : putstring FILEOUT "\n\n\nRunning:\n\n"; 1788 : make_triples' OL Points )(*;*) 1789 : 1790 : 1791 : (* val _ = System.Control.Profile.profiling := false; *) 1792 : 1793 : 1794 : (* val _ = use "RT_calc"; *) 1795 : (* simple routines to take camera and model triples and *) 1796 : (* calculate the angles, rotation matrix and translation *) 1797 : (* vectors for the triples the points represent. *) 1798 : 1799 : (* exported functions will be gen_angles, Rot_Trans and apply_RT *) 1800 : 1801 : 1802 : 1803 : (* find_camera_vectors model_triple camera_triple *) 1804 : (* calculate the camera vectors from the model and camera systems *) 1805 : 1806 : 1807 : 1808 : 1809 : local 1810 : fun calc_x_ys (a,b,c,d,e,f) x = 1811 : let val g = (a+b-c)/a 1812 : val y = (g*(1.0+x*x-2.0*d*x) - 2.0 +(2.0*d*x))/(2.0*(f*x-e)) 1813 : in 1814 : (x,y) 1815 : 1816 : end 1817 : fun calc_lmn (a,b,c,d,e,f) (x,y) = 1818 : let val l = sqrt( (a/(1.0+x*x-2.0*d*x) ) ) 1819 : val m = l*x 1820 : val n = l*y 1821 : in 1822 : (l,m,n) 1823 : end 1824 : fun calc_roots (a,b,c,d,e,f) = 1825 : let val g = (a+b-c)/a 1826 : val q4 = (b/a)*(f*f) - (g*g)/4.0 1827 : val q3 = (~2.0*b/a)*((d*f*f)+(e*f)) - (g*(d-(e*f)-(g*d))) 1828 : val q2 = ((b-a)/a)*f*f+(4.0*(b/a)*d*e*f)+(b/a)*e*e-(d*d)+2.0* 1829 : d*e*f - (g* (e*e+2.0*d*e*f-1.0-(2.0*d*d)+(g/2.0)* 1830 : (1.0+2.0*d*d))) 1831 : val q1 = (~( (2.0*(b/a)*d*e*e)+(2.0*(b/a)*e*f)+(2.0*d*e*e)-(2.0*d)) 1832 : - (g*( (3.0*d)-(2.0*d*e*e)-(e*f)-(g*d))) ) 1833 : val q0 = ((b/a)*e*e)+(e*e)-1.0-(g*( (e*e)-1.0+(g/4.0))) 1834 : in 1835 : quartic ((q3/q4),(q2/q4),(q1/q4),(q0/q4)) 1836 : end 1837 : 1838 : 1839 : (* ok try to be clever and use map and compose with the above functions *) 1840 : (* prepare asbestos gloves just in case *) 1841 : (* returns a list of possible vectors may be empty *) 1842 : in 1843 : fun find_camera_vectors model_triple camera_triple = 1844 : let 1845 : val a_to_f = T_calc_atof model_triple camera_triple 1846 : in 1847 : map ((T_calc_single_primes camera_triple) o (calc_lmn a_to_f) o 1848 : (calc_x_ys a_to_f) ) (calc_roots a_to_f) 1849 : end 1850 : end(*;*) 1851 : 1852 : 1853 : (* Rot_Trans model_triple camera_triple *) 1854 : (* for a given model and camera triple calculate the rotation matrix *) 1855 : (* and the translation vector for the transformation *) 1856 : (* returns a list (possibly empty) of (Matrix,Vector) pairs *) 1857 : fun show_triples model_triple camera_triple = 1858 : (putstring FILEOUT "model_triple:::"; 1859 : show_triple(model_triple); 1860 : putstring FILEOUT "\nscene_triple:::"; 1861 : show_triple(camera_triple) )(*;*) 1862 : 1863 : fun calc_T_vec (M:Triple) ((C:Triple),(R:Matrix)) = 1864 : V_sub (T_u C) (apply_R_to_V R (T_u M) ) (*;*) 1865 : 1866 : (* timing on or off? *) 1867 : (* 1868 : val _ = if timing_wanted then ( (System.Control.Profile.profiling := true)) else (); 1869 : *) 1870 : 1871 : 1872 : fun Rot_Trans camera_triple model_triple = 1873 : let 1874 : val N_camera = find_camera_vectors model_triple camera_triple 1875 : val o_model_triple = T_orthonormal model_triple 1876 : val o_N_camera = map (T_orthonormal) N_camera 1877 : val R_mats = map (matrix o_model_triple) o_N_camera 1878 : val T_vecs = map (calc_T_vec model_triple ) (combine (N_camera,R_mats)) 1879 : in 1880 : combine( R_mats,T_vecs) 1881 : end(*;*) 1882 : 1883 : (* val _ = System.Control.Profile.profiling := false; *) 1884 : 1885 : 1886 : 1887 : (* gen_angles model_triple camera_triple *) 1888 : (* for a given model and camera triple calculate the angles *) 1889 : (* of the rotation matrix and return them in degrees *) 1890 : 1891 : fun r_to_d x = 360.0/(2.0*3.14159) * x(*;*) 1892 : fun rad_to_deg (a:real,b:real,c:real) = (r_to_d a,r_to_d b, r_to_d c)(*;*) 1893 : fun gen_angles camera_triple model_triple = 1894 : let 1895 : val N_camera = find_camera_vectors model_triple camera_triple 1896 : val o_model_triple = T_orthonormal model_triple 1897 : val o_N_camera = map (T_orthonormal) N_camera 1898 : in 1899 : map ( rad_to_deg o calc_angles o (matrix o_model_triple)) o_N_camera 1900 : end(*;*) 1901 : 1902 : 1903 : fun apply_R_T M T V:Vector = 1904 : V_add T (apply_R_to_V M V) (*;*) 1905 : 1906 : fun display_angles (FILE:outstream) (M:Matrix) = 1907 : let val (Psi,Theta,Phi) = (rad_to_deg o calc_angles) M 1908 : in 1909 : ( 1910 : putstring FILE "\nAngles Psi = "; 1911 : putreal FILE Psi 2; 1912 : putstring FILE "\tTheta = "; 1913 : putreal FILE Theta 2; 1914 : putstring FILE "\tPhi = "; 1915 : putreal FILE Phi 2; 1916 : putstring FILE "\n" 1917 : ) 1918 : end(*;*) 1919 : 1920 : 1921 : 1922 : (* val _ = use "scoring"; *) 1923 : (* THIS WAS A NICE EFFICIENT BUBBLE SORT BUT ITS NOW AN INSERTION SORT 1924 : local 1925 : exception e_cut 1926 : fun cut 0 xs = (nil,xs) 1927 : | cut n nil = raise e_cut 1928 : | cut n (x::xs) = 1929 : let val(ys,zs) = cut (n-1) xs 1930 : in 1931 : (x::ys,zs) 1932 : end 1933 : 1934 : fun ordered ( (d1:real,_) ,(d2:real,_) ) = 1935 : d1 < d2 1936 : 1937 : fun merge nil ys = ys 1938 : | merge xs nil = xs 1939 : | merge (x::xs) (y::ys) = 1940 : if ordered(x,y) 1941 : then 1942 : (x:: (merge xs (y::ys)) ) 1943 : else 1944 : (y:: (merge (x::xs) ys) ) 1945 : 1946 : fun order' nil = nil 1947 : | order' [x] = [x] 1948 : | order' xs = 1949 : let val (ys,zs) = cut (len xs div 2) xs 1950 : in 1951 : merge (order' ys) (order' zs) 1952 : end 1953 : in 1954 : fun order nil = nil 1955 : | order ( (PtA,Dist_list)::Rest) = 1956 : (PtA, order'(Dist_list)) :: (order Rest) 1957 : end(*;*) 1958 : *) 1959 : local 1960 : fun ordered ( (d1:real,_) ,(d2:real,_) ) = d1 <= d2 1961 : 1962 : fun insert (x,nil) = x::nil 1963 : | insert (x,yys as y::ys) = 1964 : if ordered(x,y) 1965 : then x::yys 1966 : else y:: insert(x,ys) 1967 : 1968 : fun order' nil = nil 1969 : | order' (x::xs) = insert (x, order' xs) 1970 : in 1971 : fun order nil = nil 1972 : | order ( (PtA,Dist_list)::Rest) = 1973 : (PtA, (order' Dist_list )) :: (order Rest) 1974 : end(*;*) 1975 : 1976 : fun distance_list _ _ nil = nil 1977 : | distance_list Vec Type (point(bNum,bVec,bType)::Rest) = 1978 : if Type = bType 1979 : then 1980 : if (V_dist Vec bVec) < THRESHOLD 1981 : then 1982 : ( ( (V_dist Vec bVec),bNum ) :: distance_list Vec Type Rest) 1983 : else 1984 : distance_list Vec Type Rest 1985 : else 1986 : distance_list Vec Type Rest(*;*) 1987 : 1988 : 1989 : 1990 : fun get_nearness_list nil _ = nil 1991 : | get_nearness_list (point(Num,Vec,Type)::Rest) ptbs = 1992 : let val D_list = distance_list Vec Type ptbs 1993 : val (x,y,z) = V_tuple Vec 1994 : in 1995 : if D_list = nil 1996 : then 1997 : get_nearness_list Rest ptbs 1998 : else 1999 : ( (Num, D_list ) :: (get_nearness_list Rest ptbs) ) 2000 : end(*;*) 2001 : 2002 : fun remove_best_d nil _ = nil 2003 : | remove_best_d ( (Pta,Dist_Ptb)::Rest) PtA = 2004 : if PtA = Pta 2005 : then 2006 : Rest 2007 : else 2008 : ( (Pta,Dist_Ptb) :: (remove_best_d Rest PtA) )(*;*) 2009 : 2010 : fun remove_cleared' _ nil = nil 2011 : | remove_cleared' PtB ( (dist,Ptb) ::Rest) = 2012 : if PtB = Ptb 2013 : then 2014 : remove_cleared' PtB Rest 2015 : else 2016 : ( (dist,Ptb):: (remove_cleared' PtB Rest) )(*;*) 2017 : 2018 : fun remove_cleared_points _ nil = nil 2019 : | remove_cleared_points PtB (( Pta,Dist_Ptb) :: Rest) = 2020 : let val clear_list = remove_cleared' PtB Dist_Ptb 2021 : in 2022 : if clear_list = nil 2023 : then 2024 : remove_cleared_points PtB Rest 2025 : else 2026 : ( (Pta,clear_list) :: (remove_cleared_points PtB Rest) ) 2027 : end(*;*) 2028 : 2029 : 2030 : fun get_smallest_d nil smallest = smallest 2031 : | get_smallest_d ((Pta:int,( (Din:real,Ptb:int)::_))::Rest) (Pasmall,Dsmall,Pbsmall) = 2032 : if Din < Dsmall 2033 : then 2034 : get_smallest_d Rest (Pta,Din,Ptb) 2035 : else 2036 : get_smallest_d Rest (Pasmall,Dsmall,Pbsmall)(*;*) 2037 : 2038 : 2039 : (* reduce nearest list to absolute nearest neighbout list *) 2040 : fun reduce nil = nil 2041 : | reduce Pt_dist_list = 2042 : let val (PtA,dist,PtB) = get_smallest_d Pt_dist_list (0,99E3,0) 2043 : val Pt_dist_less_closest = remove_best_d Pt_dist_list PtA 2044 : val clean_Pt_dist = remove_cleared_points PtB Pt_dist_less_closest 2045 : in 2046 : ((PtA,dist,PtB) :: (reduce clean_Pt_dist)) 2047 : end(*;*) 2048 : 2049 : fun sum_distances nil = 0.0 2050 : | sum_distances ( (_,Dist,_)::Rest) = 2051 : Dist + (sum_distances Rest)(*;*) 2052 : 2053 : fun Point_score List1 List2 = 2054 : let val ptas = POINT_KILL List1 2055 : val ptbs = POINT_KILL List2 2056 : val distance_pairs = get_nearness_list ptas ptbs 2057 : val sorted_distance_pairs = order distance_pairs 2058 : val nearest_neighbours_list = reduce sorted_distance_pairs 2059 : in 2060 : 2061 : (len nearest_neighbours_list, sum_distances nearest_neighbours_list, 2062 : nearest_neighbours_list) 2063 : 2064 : end(*;*) 2065 : 2066 : 2067 : 2068 : (* val _ = use "calcs_and_ranking"; *) 2069 : (* now at this stage we have triple and points sets from both the *) 2070 : (* model and the scene. The next step is to calculate the Rot and *) 2071 : (* Translation parameters, apply the RT and perspective transformation *) 2072 : (* to the Model points and Rank them against the scene points *) 2073 : 2074 : (* transform points with Rotation and Translation and then apply *) 2075 : (* perspective transformation to produce 2d point on image plane *) 2076 : fun display_nearest (FILE:outstream) nil = 2077 : putstring FILE "\n\nThat's all folks!" 2078 : | display_nearest FILE ( (PtA,dist,PtB)::Rest) = 2079 : ( putstring FILE "\nNo. "; 2080 : putint FILE PtA; 2081 : putstring FILE " is "; 2082 : putreal FILE dist 9; 2083 : putstring FILE "mm from No. "; 2084 : putint FILE PtB; 2085 : display_nearest FILE Rest 2086 : )(*;*) 2087 : 2088 : (**) 2089 : fun display_final_result (FILE:outstream) (num:int,score:real,NN_list,(M,T)) (part_num_tried:int) = 2090 : let val (x,y,z) = V_tuple T 2091 : in 2092 : (putstring FILE "\nAfter ALL "; 2093 : putint FILE part_num_tried; 2094 : putstring FILE " Transformations have been tried\n"; 2095 : putstring FILE "The best is :- "; 2096 : putstring FILE "\n Number of points matching: "; 2097 : putint FILE num; 2098 : putstring FILE "\n For a score of: "; 2099 : putreal FILE score 9; 2100 : putstring FILE "\n\nRotation Matrix"; 2101 : print_matrix FILE M; 2102 : putstring FILE "\n\n"; 2103 : display_angles FILE M; 2104 : putstring FILE "Translation Vector"; 2105 : putstring FILE " "; 2106 : putreal FILE x 5; 2107 : putstring FILE " "; 2108 : putreal FILE y 5; 2109 : putstring FILE " "; 2110 : putreal FILE z 5; 2111 : putstring FILE " \n"; 2112 : display_nearest FILE NN_list 2113 : ) 2114 : end(*;*) 2115 : 2116 : 2117 : (* COMPARE SCORES *) 2118 : fun perfect_score ((Num:int,_,_,_),Min_num) = 2119 : Num > Min_num(*;*) 2120 : (* should be = *) 2121 : 2122 : fun compare_scores (Best_Number:int,Best_Score:real,Best_pt_pt,Best_MT) (Number,Score,pt_pt,MT) = 2123 : ( 2124 : if Best_Number > Number 2125 : then (Best_Number,Best_Score,Best_pt_pt,Best_MT) 2126 : else if Best_Number = Number 2127 : then if Best_Score < Score 2128 : then (Best_Number,Best_Score,Best_pt_pt,Best_MT) 2129 : else (Number,Score,pt_pt,MT) 2130 : else 2131 : (Number,Score,pt_pt,MT) 2132 : )(*;*) 2133 : 2134 : 2135 : (* SCORE NEAREST NEIGHBOUR FOR POINTS SETS *) 2136 : (* call the point scoring function with the shortest point set at the *) 2137 : (* front to minimise calculation time *) 2138 : 2139 : fun score_nearest_neighbour Model_points Camera_points = 2140 : if 2141 : Point_len(Model_points) > Point_len(Camera_points) 2142 : then 2143 : Point_score Camera_points Model_points 2144 : else 2145 : Point_score Model_points Camera_points(*;*) 2146 : 2147 : 2148 : (* GEN_M_T *) 2149 : (* FOR EACH camera triple and ALL model triples generate the M_T *) 2150 : (* tuples and flatten the list so produced *) 2151 : 2152 : fun gen_M_T Camera_triple Model_triples = 2153 : flatten ( map (Rot_Trans Camera_triple) Model_triples )(*;*) 2154 : 2155 : 2156 : 2157 : (* FOR EACH SCENE TRIPLE AND MODEL TRIPLES *) 2158 : (* add an 0 for the number of matrices tried, and a zero score matrix *) 2159 : (* to start the process then continue (eventually returns best score and*) 2160 : (* MT and number of Matces tried *) 2161 : 2162 : (* timing wanted, on or off? *) 2163 : (* 2164 : val _ = if timing_wanted then ( (System.Control.Profile.profiling := true)) else (); 2165 : *) 2166 : 2167 : 2168 : fun check_Mats'(nil,_,_,Best_matrix) = Best_matrix 2169 : | check_Mats'( (MT::RestMT),Mpoints,Cpoints,Best_to_date)= 2170 : let val (New_model_points,bound_box_size) = Point_transform Mpoints MT 2171 : in 2172 : if (bound_box_size > MAX_BOUNDING_BOX) 2173 : orelse (bound_box_size < MIN_BOUNDING_BOX) 2174 : then 2175 : (* return zer matrix *) 2176 : check_Mats'(RestMT, Mpoints, Cpoints, Best_to_date) 2177 : else 2178 : ( let val (Num,Score,Best_pt_pt) = 2179 : score_nearest_neighbour New_model_points Cpoints 2180 : in 2181 : check_Mats'(RestMT, Mpoints, Cpoints, 2182 : ( compare_scores Best_to_date (Num,Score,Best_pt_pt,MT) ) ) 2183 : end 2184 : ) 2185 : end(*;*) 2186 : 2187 : (* val _ = System.Control.Profile.profiling:=false; *) 2188 : 2189 : 2190 : 2191 : local 2192 : fun check_Mats(Mat_trans,Mpoints,Cpoints) = 2193 : check_Mats'(Mat_trans,Mpoints,Cpoints,(0,99E3,[],(Matrix_zero(),V_zero())))(*;*) 2194 : 2195 : fun calc_one(Mtriples,Mpoints,Ctriple,Cpoints,Num_tried) = 2196 : let val Mat_trans = gen_M_T Ctriple Mtriples 2197 : val Best_local = check_Mats(Mat_trans,Mpoints,Cpoints) 2198 : in 2199 : ( putstring FILEOUT "\nNumber of transformations tried = "; 2200 : putint FILEOUT Num_tried; 2201 : putstring FILEOUT "\n"; 2202 : 2203 : (Num_tried + (len Mat_trans) , Best_local ) 2204 : ) 2205 : end(*;*) 2206 : 2207 : fun Imin (a:int,b:int) = if a

 root@smlnj-gforge.cs.uchicago.edu ViewVC Help Powered by ViewVC 1.0.0