├── README.md
├── array1.h
├── array2.h
├── array2_utils.h
├── fluidsim.cpp
├── fluidsim.h
├── gluvi.cpp
├── gluvi.h
├── main.cpp
├── openglutils.cpp
├── openglutils.h
├── pcgsolver
    ├── blas_wrapper.h
    ├── pcg_solver.h
    └── sparse_matrix.h
├── util.h
└── vec.h


/README.md:
--------------------------------------------------------------------------------
1 | VariationalViscosity2D
2 | ======================
3 | 
4 | A 2D implementation of the SCA 2008 paper "Accurate Viscous Free Surfaces[...]" by Batty & Bridson.


--------------------------------------------------------------------------------
/array1.h:
--------------------------------------------------------------------------------
  1 | #ifndef ARRAY1_H
  2 | #define ARRAY1_H
  3 | 
  4 | #include <algorithm>
  5 | #include <cassert>
  6 | #include <climits>
  7 | #include <cstdlib>
  8 | #include <iostream>
  9 | #include <stdexcept>
 10 | #include <vector>
 11 | 
 12 | // In this file:
 13 | //   Array1<T>: a dynamic 1D array for plain-old-data (not objects)
 14 | //   WrapArray1<T>: a 1D array wrapper around an existing array (perhaps objects, perhaps data)
 15 | // For the most part std::vector operations are supported, though for the Wrap version
 16 | // note that memory is never allocated/deleted and constructor/destructors are never called
 17 | // from within the class, thus only shallow copies can be made and some operations such as
 18 | // resize() and push_back() are limited.
 19 | // Note: for the most part assertions are done with assert(), not exceptions...
 20 | 
 21 | // gross template hacking to determine if a type is integral or not
 22 | struct Array1True {};
 23 | struct Array1False {};
 24 | template<typename T> struct Array1IsIntegral{ typedef Array1False type; }; // default: no (specializations to yes follow)
 25 | template<> struct Array1IsIntegral<bool>{ typedef Array1True type; };
 26 | template<> struct Array1IsIntegral<char>{ typedef Array1True type; };
 27 | template<> struct Array1IsIntegral<signed char>{ typedef Array1True type; };
 28 | template<> struct Array1IsIntegral<unsigned char>{ typedef Array1True type; };
 29 | template<> struct Array1IsIntegral<short>{ typedef Array1True type; };
 30 | template<> struct Array1IsIntegral<unsigned short>{ typedef Array1True type; };
 31 | template<> struct Array1IsIntegral<int>{ typedef Array1True type; };
 32 | template<> struct Array1IsIntegral<unsigned int>{ typedef Array1True type; };
 33 | template<> struct Array1IsIntegral<long>{ typedef Array1True type; };
 34 | template<> struct Array1IsIntegral<unsigned long>{ typedef Array1True type; };
 35 | template<> struct Array1IsIntegral<long long>{ typedef Array1True type; };
 36 | template<> struct Array1IsIntegral<unsigned long long>{ typedef Array1True type; };
 37 | 
 38 | //============================================================================
 39 | template<typename T>
 40 | struct Array1
 41 | {
 42 |    // STL-friendly typedefs
 43 | 
 44 |    typedef T* iterator;
 45 |    typedef const T* const_iterator;
 46 |    typedef unsigned long size_type;
 47 |    typedef long difference_type;
 48 |    typedef T& reference;
 49 |    typedef const T& const_reference;
 50 |    typedef T value_type;
 51 |    typedef T* pointer;
 52 |    typedef const T* const_pointer;
 53 |    typedef std::reverse_iterator<iterator> reverse_iterator;
 54 |    typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
 55 | 
 56 |    // the actual representation
 57 | 
 58 |    unsigned long n;
 59 |    unsigned long max_n;
 60 |    T* data;
 61 | 
 62 |    // STL vector's interface, with additions, but only valid when used with plain-old-data
 63 | 
 64 |    Array1(void)
 65 |       : n(0), max_n(0), data(0)
 66 |    {}
 67 | 
 68 |    // note: default initial values are zero
 69 |    Array1(unsigned long n_)
 70 |       : n(0), max_n(0), data(0)
 71 |    {
 72 |       if(n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
 73 |       data=(T*)std::calloc(n_, sizeof(T));
 74 |       if(!data) throw std::bad_alloc();
 75 |       n=n_;
 76 |       max_n=n_;
 77 |    }
 78 | 
 79 |    Array1(unsigned long n_, const T& value)
 80 |       : n(0), max_n(0), data(0)
 81 |    {
 82 |       if(n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
 83 |       data=(T*)std::calloc(n_, sizeof(T));
 84 |       if(!data) throw std::bad_alloc();
 85 |       n=n_;
 86 |       max_n=n_;
 87 |       for(unsigned long i=0; i<n; ++i) data[i]=value;
 88 |    }
 89 | 
 90 |    Array1(unsigned long n_, const T& value, unsigned long max_n_)
 91 |       : n(0), max_n(0), data(0)
 92 |    {
 93 |       assert(n_<=max_n_);
 94 |       if(max_n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
 95 |       data=(T*)std::calloc(max_n_, sizeof(T));
 96 |       if(!data) throw std::bad_alloc();
 97 |       n=n_;
 98 |       max_n=max_n_;
 99 |       for(unsigned long i=0; i<n; ++i) data[i]=value;
100 |    }
101 | 
102 |    Array1(unsigned long n_, const T* data_)
103 |       : n(0), max_n(0), data(0)
104 |    {
105 |       if(n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
106 |       data=(T*)std::calloc(n_, sizeof(T));
107 |       if(!data) throw std::bad_alloc();
108 |       n=n_;
109 |       max_n=n_;
110 |       assert(data_);
111 |       std::memcpy(data, data_, n*sizeof(T));
112 |    }
113 | 
114 |    Array1(unsigned long n_, const T* data_, unsigned long max_n_)
115 |       : n(0), max_n(0), data(0)
116 |    {
117 |       assert(n_<=max_n_);
118 |       if(max_n_>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
119 |       data=(T*)std::calloc(max_n_, sizeof(T));
120 |       if(!data) throw std::bad_alloc();
121 |       max_n=max_n_;
122 |       n=n_;
123 |       assert(data_);
124 |       std::memcpy(data, data_, n*sizeof(T));
125 |    }
126 | 
127 |    Array1(const Array1<T> &x)
128 |       : n(0), max_n(0), data(0)
129 |    {
130 |       data=(T*)std::malloc(x.n*sizeof(T));
131 |       if(!data) throw std::bad_alloc();
132 |       n=x.n;
133 |       max_n=x.n;
134 |       std::memcpy(data, x.data, n*sizeof(T));
135 |    }
136 | 
137 |    ~Array1(void)
138 |    {
139 |       std::free(data);
140 | #ifndef NDEBUG
141 |       data=0;
142 |       n=max_n=0;
143 | #endif
144 |    }
145 | 
146 |    const T& operator[](unsigned long i) const
147 |    { return data[i]; }
148 | 
149 |    T& operator[](unsigned long i)
150 |    { return data[i]; }
151 | 
152 |    // these are range-checked (in debug mode) versions of operator[], like at()
153 |    const T& operator()(unsigned long i) const
154 |    {
155 |       assert(i<n);
156 |       return data[i];
157 |    }
158 | 
159 |    T& operator()(unsigned long i)
160 |    {
161 |       assert(i<n);
162 |       return data[i];
163 |    }
164 | 
165 |    Array1<T>& operator=(const Array1<T>& x)
166 |    {
167 |       if(max_n<x.n){
168 |          T* new_data=(T*)std::malloc(x.n*sizeof(T));
169 |          if(!new_data) throw std::bad_alloc();
170 |          std::free(data);
171 |          data=new_data;
172 |          max_n=x.n;
173 |       }
174 |       n=x.n;
175 |       std::memcpy(data, x.data, n*sizeof(T));
176 |       return *this;
177 |    }
178 | 
179 |    bool operator==(const Array1<T>& x) const
180 |    {
181 |       if(n!=x.n) return false;
182 |       for(unsigned long i=0; i<n; ++i) if(!(data[i]==x.data[i])) return false;
183 |       return true;
184 |    }
185 |  
186 |    bool operator!=(const Array1<T>& x) const
187 |    {
188 |       if(n!=x.n) return true;
189 |       for(unsigned long i=0; i<n; ++i) if(data[i]!=x.data[i]) return true;
190 |       return false;
191 |    }
192 | 
193 |    bool operator<(const Array1<T>& x) const
194 |    {
195 |       for(unsigned long i=0; i<n && i<x.n; ++i){
196 |          if(data[i]<x[i]) return true;
197 |          else if(x[i]<data[i]) return false;
198 |       }
199 |       return n<x.n;
200 |    }
201 | 
202 |    bool operator>(const Array1<T>& x) const
203 |    {
204 |       for(unsigned long i=0; i<n && i<x.n; ++i){
205 |          if(data[i]>x[i]) return true;
206 |          else if(x[i]>data[i]) return false;
207 |       }
208 |       return n>x.n;
209 |    }
210 | 
211 |    bool operator<=(const Array1<T>& x) const
212 |    {
213 |       for(unsigned long i=0; i<n && i<x.n; ++i){
214 |          if(data[i]<x[i]) return true;
215 |          else if(x[i]<data[i]) return false;
216 |       }
217 |       return n<=x.n;
218 |    }
219 | 
220 |    bool operator>=(const Array1<T>& x) const
221 |    {
222 |       for(unsigned long i=0; i<n && i<x.n; ++i){
223 |          if(data[i]>x[i]) return true;
224 |          else if(x[i]>data[i]) return false;
225 |       }
226 |       return n>=x.n;
227 |    }
228 | 
229 |    void add_unique(const T& value)
230 |    {
231 |       for(unsigned long i=0; i<n; ++i) if(data[i]==value) return;
232 |       if(n==max_n) grow();
233 |       data[n++]=value;
234 |    }
235 | 
236 |    void assign(const T& value)
237 |    { for(unsigned long i=0; i<n; ++i) data[i]=value; }
238 | 
239 |    void assign(unsigned long num, const T& value)
240 |    { fill(num, value); } 
241 | 
242 |    // note: copydata may not alias this array's data, and this should not be
243 |    // used when T is a full object (which defines its own copying operation)
244 |    void assign(unsigned long num, const T* copydata)
245 |    {
246 |       assert(num==0 || copydata);
247 |       if(num>max_n){
248 |          if(num>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
249 |          std::free(data);
250 |          data=(T*)std::malloc(num*sizeof(T));
251 |          if(!data) throw std::bad_alloc();
252 |          max_n=num;
253 |       }
254 |       n=num;
255 |       std::memcpy(data, copydata, n*sizeof(T));
256 |    }
257 | 
258 |    template<typename InputIterator>
259 |    void assign(InputIterator first, InputIterator last)
260 |    { assign_(first, last, typename Array1IsIntegral<InputIterator>::type()); }
261 | 
262 |    template<typename InputIterator>
263 |    void assign_(InputIterator first, InputIterator last, Array1True check)
264 |    { fill(first, last); }
265 | 
266 |    template<typename InputIterator>
267 |    void assign_(InputIterator first, InputIterator last, Array1False check)
268 |    {
269 |       unsigned long i=0;
270 |       InputIterator p=first;
271 |       for(; p!=last; ++p, ++i){
272 |          if(i==max_n) grow();
273 |          data[i]=*p;
274 |       }
275 |       n=i;
276 |    }
277 | 
278 |    const T& at(unsigned long i) const
279 |    {
280 |       assert(i<n);
281 |       return data[i];
282 |    }
283 | 
284 |    T& at(unsigned long i)
285 |    {
286 |       assert(i<n);
287 |       return data[i];
288 |    }
289 | 
290 |    const T& back(void) const
291 |    { 
292 |       assert(data && n>0);
293 |       return data[n-1];
294 |    }
295 | 
296 |    T& back(void)
297 |    {
298 |       assert(data && n>0);
299 |       return data[n-1];
300 |    }
301 | 
302 |    const T* begin(void) const
303 |    { return data; }
304 | 
305 |    T* begin(void)
306 |    { return data; }
307 | 
308 |    unsigned long capacity(void) const
309 |    { return max_n; }
310 | 
311 |    void clear(void)
312 |    {
313 |       std::free(data);
314 |       data=0;
315 |       max_n=0;
316 |       n=0;
317 |    }
318 | 
319 |    bool empty(void) const
320 |    { return n==0; }
321 | 
322 |    const T* end(void) const
323 |    { return data+n; }
324 | 
325 |    T* end(void)
326 |    { return data+n; }
327 | 
328 |    void erase(unsigned long index)
329 |    {
330 |       assert(index<n);
331 |       for(unsigned long i=index; i<n-1; ++i)
332 |          data[i]=data[i-1];
333 |       pop_back();
334 |    }
335 | 
336 |    void fill(unsigned long num, const T& value)
337 |    {
338 |       if(num>max_n){
339 |          if(num>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
340 |          std::free(data);
341 |          data=(T*)std::malloc(num*sizeof(T));
342 |          if(!data) throw std::bad_alloc();
343 |          max_n=num;
344 |       }
345 |       n=num;
346 |       for(unsigned long i=0; i<n; ++i) data[i]=value;
347 |    }
348 | 
349 |    const T& front(void) const
350 |    {
351 |       assert(n>0);
352 |       return *data;
353 |    }
354 | 
355 |    T& front(void)
356 |    {
357 |       assert(n>0);
358 |       return *data;
359 |    }
360 | 
361 |    void grow(void)
362 |    {
363 |       unsigned long new_size=(max_n*sizeof(T)<ULONG_MAX/2 ? 2*max_n+1 : ULONG_MAX/sizeof(T));
364 |       T *new_data=(T*)std::realloc(data, new_size*sizeof(T));
365 |       if(!new_data) throw std::bad_alloc();
366 |       data=new_data;
367 |       max_n=new_size;
368 |    }
369 | 
370 |    void insert(unsigned long index, const T& entry)
371 |    {
372 |       assert(index<=n);
373 |       push_back(back());
374 |       for(unsigned long i=n-1; i>index; --i)
375 |          data[i]=data[i-1];
376 |       data[index]=entry;
377 |    }
378 | 
379 |    unsigned long max_size(void) const
380 |    { return ULONG_MAX/sizeof(T); }
381 | 
382 |    void pop_back(void)
383 |    {
384 |       assert(n>0);
385 |       --n;
386 |    }
387 | 
388 |    void push_back(const T& value)
389 |    {
390 |       if(n==max_n) grow();
391 |       data[n++]=value;
392 |    }
393 | 
394 |    reverse_iterator rbegin(void)
395 |    { return reverse_iterator(end()); }
396 | 
397 |    const_reverse_iterator rbegin(void) const
398 |    { return const_reverse_iterator(end()); }
399 | 
400 |    reverse_iterator rend(void)
401 |    { return reverse_iterator(begin()); }
402 | 
403 |    const_reverse_iterator rend(void) const
404 |    { return const_reverse_iterator(begin()); }
405 | 
406 |    void reserve(unsigned long r)
407 |    {
408 |       if(r>ULONG_MAX/sizeof(T)) throw std::bad_alloc();
409 |       T *new_data=(T*)std::realloc(data, r*sizeof(T));
410 |       if(!new_data) throw std::bad_alloc();
411 |       data=new_data;
412 |       max_n=r;
413 |    }
414 | 
415 |    void resize(unsigned long n_)
416 |    {
417 |       if(n_>max_n) reserve(n_);
418 |       n=n_;
419 |    }
420 | 
421 |    void resize(unsigned long n_, const T& value)
422 |    {
423 |       if(n_>max_n) reserve(n_);
424 |       if(n<n_) for(unsigned long i=n; i<n_; ++i) data[i]=value;
425 |       n=n_;
426 |    }
427 | 
428 |    void set_zero(void)
429 |    { std::memset(data, 0, n*sizeof(T)); }
430 | 
431 |    unsigned long size(void) const
432 |    { return n; }
433 | 
434 |    void swap(Array1<T>& x)
435 |    {
436 |       std::swap(n, x.n);
437 |       std::swap(max_n, x.max_n);
438 |       std::swap(data, x.data);
439 |    }
440 | 
441 |    // resize the array to avoid wasted space, without changing contents
442 |    // (Note: realloc, at least on some platforms, will not do the trick)
443 |    void trim(void)
444 |    {
445 |       if(n==max_n) return;
446 |       T *new_data=(T*)std::malloc(n*sizeof(T));
447 |       if(!new_data) return;
448 |       std::memcpy(new_data, data, n*sizeof(T));
449 |       std::free(data);
450 |       data=new_data;
451 |       max_n=n;
452 |    }
453 | };
454 | 
455 | // some common arrays
456 | 
457 | typedef Array1<double>             Array1d;
458 | typedef Array1<float>              Array1f;
459 | typedef Array1<long long>          Array1ll;
460 | typedef Array1<unsigned long long> Array1ull;
461 | typedef Array1<int>                Array1i;
462 | typedef Array1<unsigned int>       Array1ui;
463 | typedef Array1<short>              Array1s;
464 | typedef Array1<unsigned short>     Array1us;
465 | typedef Array1<char>               Array1c;
466 | typedef Array1<unsigned char>      Array1uc;
467 | 
468 | //============================================================================
469 | template<typename T>
470 | struct WrapArray1
471 | {
472 |    // STL-friendly typedefs
473 | 
474 |    typedef T* iterator;
475 |    typedef const T* const_iterator;
476 |    typedef unsigned long size_type;
477 |    typedef long difference_type;
478 |    typedef T& reference;
479 |    typedef const T& const_reference;
480 |    typedef T value_type;
481 |    typedef T* pointer;
482 |    typedef const T* const_pointer;
483 |    typedef std::reverse_iterator<iterator> reverse_iterator;
484 |    typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
485 | 
486 |    // the actual representation
487 | 
488 |    unsigned long n;
489 |    unsigned long max_n;
490 |    T* data;
491 | 
492 |    // most of STL vector's interface, with a few changes
493 | 
494 |    WrapArray1(void)
495 |       : n(0), max_n(0), data(0)
496 |    {}
497 | 
498 |    WrapArray1(unsigned long n_, T* data_)
499 |       : n(n_), max_n(n_), data(data_)
500 |    { assert(data || max_n==0); }
501 | 
502 |    WrapArray1(unsigned long n_, T* data_, unsigned long max_n_)
503 |       : n(n_), max_n(max_n_), data(data_)
504 |    {
505 |       assert(n<=max_n);
506 |       assert(data || max_n==0);
507 |    }
508 | 
509 |    // Allow for simple shallow copies of existing arrays
510 |    // Note that if the underlying arrays change where their data is, the WrapArray may be screwed up
511 | 
512 |    WrapArray1(Array1<T>& a)
513 |       : n(a.n), max_n(a.max_n), data(a.data)
514 |    {}
515 | 
516 |    WrapArray1(std::vector<T>& a)
517 |       : n(a.size()), max_n(a.capacity()), data(&a[0])
518 |    {}
519 | 
520 |    void init(unsigned long n_, T* data_, unsigned long max_n_)
521 |    {
522 |       assert(n_<=max_n_);
523 |       assert(data_ || max_n_==0);
524 |       n=n_;
525 |       max_n=max_n_;
526 |       data=data_;
527 |    }
528 | 
529 |    const T& operator[](unsigned long i) const
530 |    { return data[i]; }
531 | 
532 |    T& operator[](unsigned long i)
533 |    { return data[i]; }
534 | 
535 |    // these are range-checked (in debug mode) versions of operator[], like at()
536 |    const T& operator()(unsigned long i) const
537 |    {
538 |       assert(i<n);
539 |       return data[i];
540 |    }
541 | 
542 |    T& operator()(unsigned long i)
543 |    {
544 |       assert(i<n);
545 |       return data[i];
546 |    }
547 | 
548 |    bool operator==(const WrapArray1<T>& x) const
549 |    {
550 |       if(n!=x.n) return false;
551 |       for(unsigned long i=0; i<n; ++i) if(!(data[i]==x.data[i])) return false;
552 |       return true;
553 |    }
554 |  
555 |    bool operator!=(const WrapArray1<T>& x) const
556 |    {
557 |       if(n!=x.n) return true;
558 |       for(unsigned long i=0; i<n; ++i) if(data[i]!=x.data[i]) return true;
559 |       return false;
560 |    }
561 | 
562 |    bool operator<(const WrapArray1<T>& x) const
563 |    {
564 |       for(unsigned long i=0; i<n && i<x.n; ++i){
565 |          if(data[i]<x[i]) return true;
566 |          else if(x[i]<data[i]) return false;
567 |       }
568 |       return n<x.n;
569 |    }
570 | 
571 |    bool operator>(const WrapArray1<T>& x) const
572 |    {
573 |       for(unsigned long i=0; i<n && i<x.n; ++i){
574 |          if(data[i]>x[i]) return true;
575 |          else if(x[i]>data[i]) return false;
576 |       }
577 |       return n>x.n;
578 |    }
579 | 
580 |    bool operator<=(const WrapArray1<T>& x) const
581 |    {
582 |       for(unsigned long i=0; i<n && i<x.n; ++i){
583 |          if(data[i]<x[i]) return true;
584 |          else if(x[i]<data[i]) return false;
585 |       }
586 |       return n<=x.n;
587 |    }
588 | 
589 |    bool operator>=(const WrapArray1<T>& x) const
590 |    {
591 |       for(unsigned long i=0; i<n && i<x.n; ++i){
592 |          if(data[i]>x[i]) return true;
593 |          else if(x[i]>data[i]) return false;
594 |       }
595 |       return n>=x.n;
596 |    }
597 | 
598 |    void add_unique(const T& value)
599 |    {
600 |       for(unsigned long i=0; i<n; ++i) if(data[i]==value) return;
601 |       assert(n<max_n);
602 |       data[n++]=value;
603 |    }
604 | 
605 |    void assign(const T& value)
606 |    { for(unsigned long i=0; i<n; ++i) data[i]=value; }
607 | 
608 |    void assign(unsigned long num, const T& value)
609 |    { fill(num, value); } 
610 | 
611 |    // note: copydata may not alias this array's data, and this should not be
612 |    // used when T is a full object (which defines its own copying operation)
613 |    void assign(unsigned long num, const T* copydata)
614 |    {
615 |       assert(num==0 || copydata);
616 |       assert(num<=max_n);
617 |       n=num;
618 |       std::memcpy(data, copydata, n*sizeof(T));
619 |    }
620 | 
621 |    template<typename InputIterator>
622 |    void assign(InputIterator first, InputIterator last)
623 |    { assign_(first, last, typename Array1IsIntegral<InputIterator>::type()); }
624 | 
625 |    template<typename InputIterator>
626 |    void assign_(InputIterator first, InputIterator last, Array1True check)
627 |    { fill(first, last); }
628 | 
629 |    template<typename InputIterator>
630 |    void assign_(InputIterator first, InputIterator last, Array1False check)
631 |    {
632 |       unsigned long i=0;
633 |       InputIterator p=first;
634 |       for(; p!=last; ++p, ++i){
635 |          assert(i<max_n);
636 |          data[i]=*p;
637 |       }
638 |       n=i;
639 |    }
640 | 
641 |    const T& at(unsigned long i) const
642 |    {
643 |       assert(i<n);
644 |       return data[i];
645 |    }
646 | 
647 |    T& at(unsigned long i)
648 |    {
649 |       assert(i<n);
650 |       return data[i];
651 |    }
652 | 
653 |    const T& back(void) const
654 |    { 
655 |       assert(data && n>0);
656 |       return data[n-1];
657 |    }
658 | 
659 |    T& back(void)
660 |    {
661 |       assert(data && n>0);
662 |       return data[n-1];
663 |    }
664 | 
665 |    const T* begin(void) const
666 |    { return data; }
667 | 
668 |    T* begin(void)
669 |    { return data; }
670 | 
671 |    unsigned long capacity(void) const
672 |    { return max_n; }
673 | 
674 |    void clear(void)
675 |    { n=0; }
676 | 
677 |    bool empty(void) const
678 |    { return n==0; }
679 | 
680 |    const T* end(void) const
681 |    { return data+n; }
682 | 
683 |    T* end(void)
684 |    { return data+n; }
685 | 
686 |    void erase(unsigned long index)
687 |    {
688 |       assert(index<n);
689 |       for(unsigned long i=index; i<n-1; ++i)
690 |          data[i]=data[i-1];
691 |       pop_back();
692 |    }
693 | 
694 |    void fill(unsigned long num, const T& value)
695 |    {
696 |       assert(num<=max_n);
697 |       n=num;
698 |       for(unsigned long i=0; i<n; ++i) data[i]=value;
699 |    }
700 | 
701 |    const T& front(void) const
702 |    {
703 |       assert(n>0);
704 |       return *data;
705 |    }
706 | 
707 |    T& front(void)
708 |    {
709 |       assert(n>0);
710 |       return *data;
711 |    }
712 | 
713 |    void insert(unsigned long index, const T& entry)
714 |    {
715 |       assert(index<=n);
716 |       push_back(back());
717 |       for(unsigned long i=n-1; i>index; --i)
718 |          data[i]=data[i-1];
719 |       data[index]=entry;
720 |    }
721 | 
722 |    unsigned long max_size(void) const
723 |    { return max_n; }
724 | 
725 |    void pop_back(void)
726 |    {
727 |       assert(n>0);
728 |       --n;
729 |    }
730 | 
731 |    void push_back(const T& value)
732 |    {
733 |       assert(n<max_n);
734 |       data[n++]=value;
735 |    }
736 | 
737 |    reverse_iterator rbegin(void)
738 |    { return reverse_iterator(end()); }
739 | 
740 |    const_reverse_iterator rbegin(void) const
741 |    { return const_reverse_iterator(end()); }
742 | 
743 |    reverse_iterator rend(void)
744 |    { return reverse_iterator(begin()); }
745 | 
746 |    const_reverse_iterator rend(void) const
747 |    { return const_reverse_iterator(begin()); }
748 | 
749 |    void reserve(unsigned long r)
750 |    { assert(r<=max_n); }
751 | 
752 |    void resize(unsigned long n_)
753 |    {
754 |       assert(n_<=max_n);
755 |       n=n_;
756 |    }
757 | 
758 |    void resize(unsigned long n_, const T& value)
759 |    {
760 |       assert(n_<=max_n);
761 |       if(n<n_) for(unsigned long i=n; i<n_; ++i) data[i]=value;
762 |       n=n_;
763 |    }
764 | 
765 |    // note: shouldn't be used when T is a full object (setting to zero may not make sense)
766 |    void set_zero(void)
767 |    { std::memset(data, 0, n*sizeof(T)); }
768 | 
769 |    unsigned long size(void) const
770 |    { return n; }
771 | 
772 |    void swap(WrapArray1<T>& x)
773 |    {
774 |       std::swap(n, x.n);
775 |       std::swap(max_n, x.max_n);
776 |       std::swap(data, x.data);
777 |    }
778 | };
779 | 
780 | // some common arrays
781 | 
782 | typedef WrapArray1<double>             WrapArray1d;
783 | typedef WrapArray1<float>              WrapArray1f;
784 | typedef WrapArray1<long long>          WrapArray1ll;
785 | typedef WrapArray1<unsigned long long> WrapArray1ull;
786 | typedef WrapArray1<int>                WrapArray1i;
787 | typedef WrapArray1<unsigned int>       WrapArray1ui;
788 | typedef WrapArray1<short>              WrapArray1s;
789 | typedef WrapArray1<unsigned short>     WrapArray1us;
790 | typedef WrapArray1<char>               WrapArray1c;
791 | typedef WrapArray1<unsigned char>      WrapArray1uc;
792 | 
793 | #endif
794 | 


--------------------------------------------------------------------------------
/array2.h:
--------------------------------------------------------------------------------
  1 | #ifndef ARRAY2_H
  2 | #define ARRAY2_H
  3 | 
  4 | #include "array1.h"
  5 | #include <algorithm>
  6 | #include <cassert>
  7 | #include <vector>
  8 | 
  9 | template<class T, class ArrayT=std::vector<T> >
 10 | struct Array2
 11 | {
 12 |    // STL-friendly typedefs
 13 | 
 14 |    typedef typename ArrayT::iterator iterator;
 15 |    typedef typename ArrayT::const_iterator const_iterator;
 16 |    typedef typename ArrayT::size_type size_type;
 17 |    typedef long difference_type;
 18 |    typedef T& reference;
 19 |    typedef const T& const_reference;
 20 |    typedef T value_type;
 21 |    typedef T* pointer;
 22 |    typedef const T* const_pointer;
 23 |    typedef typename ArrayT::reverse_iterator reverse_iterator;
 24 |    typedef typename ArrayT::const_reverse_iterator const_reverse_iterator;
 25 | 
 26 |    // the actual representation
 27 | 
 28 |    int ni, nj;
 29 |    ArrayT a;
 30 | 
 31 |    // the interface
 32 | 
 33 |    Array2(void)
 34 |       : ni(0), nj(0)
 35 |    {}
 36 | 
 37 |    Array2(int ni_, int nj_)
 38 |       : ni(ni_), nj(nj_), a(ni_*nj_)
 39 |    { assert(ni_>=0 && nj>=0); }
 40 | 
 41 |    Array2(int ni_, int nj_, ArrayT& a_)
 42 |       : ni(ni_), nj(nj_), a(a_)
 43 |    { assert(ni_>=0 && nj>=0); }
 44 | 
 45 |    Array2(int ni_, int nj_, const T& value)
 46 |       : ni(ni_), nj(nj_), a(ni_*nj_, value)
 47 |    { assert(ni_>=0 && nj>=0); }
 48 | 
 49 |    Array2(int ni_, int nj_, const T& value, size_type max_n_)
 50 |       : ni(ni_), nj(nj_), a(ni_*nj_, value, max_n_)
 51 |    { assert(ni_>=0 && nj>=0); }
 52 | 
 53 |    Array2(int ni_, int nj_, T* data_)
 54 |       : ni(ni_), nj(nj_), a(ni_*nj_, data_)
 55 |    { assert(ni_>=0 && nj>=0); }
 56 | 
 57 |    Array2(int ni_, int nj_, T* data_, size_type max_n_)
 58 |       : ni(ni_), nj(nj_), a(ni_*nj_, data_, max_n_)
 59 |    { assert(ni_>=0 && nj>=0); }
 60 | 
 61 |    template<class OtherArrayT>
 62 |    Array2(Array2<T, OtherArrayT>& other)
 63 |       : ni(other.ni), nj(other.nj), a(other.a)
 64 |    {}
 65 | 
 66 |    ~Array2(void)
 67 |    {
 68 | #ifndef NDEBUG
 69 |       ni=nj=0;
 70 | #endif
 71 |    }
 72 | 
 73 |    const T& operator()(int i, int j) const
 74 |    {
 75 |       assert(i>=0 && i<ni && j>=0 && j<nj);
 76 |       return a[i+ni*j];
 77 |    }
 78 | 
 79 |    T& operator()(int i, int j)
 80 |    {
 81 |       assert(i>=0 && i<ni && j>=0 && j<nj);
 82 |       return a[i+ni*j];
 83 |    }
 84 | 
 85 |    bool operator==(const Array2<T>& x) const
 86 |    { return ni==x.ni && nj==x.nj && a==x.a; }
 87 | 
 88 |    bool operator!=(const Array2<T>& x) const
 89 |    { return ni!=x.ni || nj!=x.nj || a!=x.a; }
 90 | 
 91 |    bool operator<(const Array2<T>& x) const
 92 |    {
 93 |       if(ni<x.ni) return true; else if(ni>x.ni) return false;
 94 |       if(nj<x.nj) return true; else if(nj>x.nj) return false;
 95 |       return a<x.a;
 96 |    }
 97 | 
 98 |    bool operator>(const Array2<T>& x) const
 99 |    {
100 |       if(ni>x.ni) return true; else if(ni<x.ni) return false;
101 |       if(nj>x.nj) return true; else if(nj<x.nj) return false;
102 |       return a>x.a;
103 |    }
104 | 
105 |    bool operator<=(const Array2<T>& x) const
106 |    {
107 |       if(ni<x.ni) return true; else if(ni>x.ni) return false;
108 |       if(nj<x.nj) return true; else if(nj>x.nj) return false;
109 |       return a<=x.a;
110 |    }
111 | 
112 |    bool operator>=(const Array2<T>& x) const
113 |    {
114 |       if(ni>x.ni) return true; else if(ni<x.ni) return false;
115 |       if(nj>x.nj) return true; else if(nj<x.nj) return false;
116 |       return a>=x.a;
117 |    }
118 | 
119 |    void assign(const T& value)
120 |    { a.assign(value); }
121 | 
122 |    void assign(int ni_, int nj_, const T& value)
123 |    {
124 |       a.assign(ni_*nj_, value);
125 |       ni=ni_;
126 |       nj=nj_;
127 |    }
128 |     
129 |    void assign(int ni_, int nj_, const T* copydata)
130 |    {
131 |       a.assign(ni_*nj_, copydata);
132 |       ni=ni_;
133 |       nj=nj_;
134 |    }
135 |     
136 |    const T& at(int i, int j) const
137 |    {
138 |       assert(i>=0 && i<ni && j>=0 && j<nj);
139 |       return a[i+ni*j];
140 |    }
141 | 
142 |    T& at(int i, int j)
143 |    {
144 |       assert(i>=0 && i<ni && j>=0 && j<nj);
145 |       return a[i+ni*j];
146 |    }
147 | 
148 |    const T& back(void) const
149 |    { 
150 |       assert(a.size());
151 |       return a.back();
152 |    }
153 | 
154 |    T& back(void)
155 |    {
156 |       assert(a.size());
157 |       return a.back();
158 |    }
159 | 
160 |    const_iterator begin(void) const
161 |    { return a.begin(); }
162 | 
163 |    iterator begin(void)
164 |    { return a.begin(); }
165 | 
166 |    size_type capacity(void) const
167 |    { return a.capacity(); }
168 | 
169 |    void clear(void)
170 |    {
171 |       a.clear();
172 |       ni=nj=0;
173 |    }
174 | 
175 |    bool empty(void) const
176 |    { return a.empty(); }
177 | 
178 |    const_iterator end(void) const
179 |    { return a.end(); }
180 | 
181 |    iterator end(void)
182 |    { return a.end(); }
183 | 
184 |    void fill(int ni_, int nj_, const T& value)
185 |    {
186 |       a.fill(ni_*nj_, value);
187 |       ni=ni_;
188 |       nj=nj_;
189 |    }
190 |     
191 |    const T& front(void) const
192 |    {
193 |       assert(a.size());
194 |       return a.front();
195 |    }
196 | 
197 |    T& front(void)
198 |    {
199 |       assert(a.size());
200 |       return a.front();
201 |    }
202 | 
203 |    size_type max_size(void) const
204 |    { return a.max_size(); }
205 | 
206 |    reverse_iterator rbegin(void)
207 |    { return reverse_iterator(end()); }
208 | 
209 |    const_reverse_iterator rbegin(void) const
210 |    { return const_reverse_iterator(end()); }
211 | 
212 |    reverse_iterator rend(void)
213 |    { return reverse_iterator(begin()); }
214 | 
215 |    const_reverse_iterator rend(void) const
216 |    { return const_reverse_iterator(begin()); }
217 | 
218 |    void reserve(int reserve_ni, int reserve_nj)
219 |    { a.reserve(reserve_ni*reserve_nj); }
220 | 
221 |    void resize(int ni_, int nj_)
222 |    {
223 |       assert(ni_>=0 && nj_>=0);
224 |       a.resize(ni_*nj_);
225 |       ni=ni_;
226 |       nj=nj_;
227 |    }
228 | 
229 |    void resize(int ni_, int nj_, const T& value)
230 |    {
231 |       assert(ni_>=0 && nj_>=0);
232 |       a.resize(ni_*nj_, value);
233 |       ni=ni_;
234 |       nj=nj_;
235 |    }
236 | 
237 |    void set_zero(void)
238 |    { a.set_zero(); }
239 | 
240 |    size_type size(void) const
241 |    { return a.size(); }
242 | 
243 |    void swap(Array2<T>& x)
244 |    {
245 |       std::swap(ni, x.ni);
246 |       std::swap(nj, x.nj);
247 |       a.swap(x.a);
248 |    }
249 | 
250 |    void trim(void)
251 |    { a.trim(); }
252 | };
253 | 
254 | // some common arrays
255 | 
256 | typedef Array2<double, Array1<double> >                         Array2d;
257 | typedef Array2<float, Array1<float> >                           Array2f;
258 | typedef Array2<long long, Array1<long long> >                   Array2ll;
259 | typedef Array2<unsigned long long, Array1<unsigned long long> > Array2ull;
260 | typedef Array2<int, Array1<int> >                               Array2i;
261 | typedef Array2<unsigned int, Array1<unsigned int> >             Array2ui;
262 | typedef Array2<short, Array1<short> >                           Array2s;
263 | typedef Array2<unsigned short, Array1<unsigned short> >         Array2us;
264 | typedef Array2<char, Array1<char> >                             Array2c;
265 | typedef Array2<unsigned char, Array1<unsigned char> >           Array2uc;
266 | 
267 | // and wrapped versions
268 | 
269 | typedef Array2<double, WrapArray1<double> >                         WrapArray2d;
270 | typedef Array2<float, WrapArray1<float> >                           WrapArray2f;
271 | typedef Array2<long long, WrapArray1<long long> >                   WrapArray2ll;
272 | typedef Array2<unsigned long long, WrapArray1<unsigned long long> > WrapArray2ull;
273 | typedef Array2<int, WrapArray1<int> >                               WrapArray2i;
274 | typedef Array2<unsigned int, WrapArray1<unsigned int> >             WrapArray2ui;
275 | typedef Array2<short, WrapArray1<short> >                           WrapArray2s;
276 | typedef Array2<unsigned short, WrapArray1<unsigned short> >         WrapArray2us;
277 | typedef Array2<char, WrapArray1<char> >                             WrapArray2c;
278 | typedef Array2<unsigned char, WrapArray1<unsigned char> >           WrapArray2uc;
279 | 
280 | #endif
281 | 


--------------------------------------------------------------------------------
/array2_utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef ARRAY2_UTILS_H
 2 | #define ARRAY2_UTILS_H
 3 | 
 4 | #include "vec.h"
 5 | #include "array2.h"
 6 | #include "util.h"
 7 | 
 8 | template<class S, class T>
 9 | T interpolate_value(const Vec<2,S>& point, const Array2<T, Array1<T> >& grid) {
10 |    int i,j;
11 |    S fx,fy;
12 | 
13 |    get_barycentric(point[0], i, fx, 0, grid.ni);
14 |    get_barycentric(point[1], j, fy, 0, grid.nj);
15 |    
16 |    return bilerp(
17 |       grid(i,j), grid(i+1,j), 
18 |       grid(i,j+1), grid(i+1,j+1), 
19 |       fx, fy);
20 | }
21 | 
22 | template<class S, class T>
23 | float interpolate_gradient(Vec<2,T>& gradient, const Vec<2,S>& point, const Array2<T, Array1<float> >& grid) {
24 |    int i,j;
25 |    S fx,fy;
26 |    get_barycentric(point[0], i, fx, 0, grid.ni);
27 |    get_barycentric(point[1], j, fy, 0, grid.nj);
28 | 
29 |    T v00 = grid(i,j);
30 |    T v01 = grid(i,j+1);
31 |    T v10 = grid(i+1,j);
32 |    T v11 = grid(i+1,j+1);
33 | 
34 |    T ddy0 = (v01 - v00);
35 |    T ddy1 = (v11 - v10);
36 | 
37 |    T ddx0 = (v10 - v00);
38 |    T ddx1 = (v11 - v01);
39 | 
40 |    gradient[0] = lerp(ddx0,ddx1,fy);
41 |    gradient[1] = lerp(ddy0,ddy1,fx);
42 |    
43 |    //may as well return value too
44 |    return bilerp(v00, v10, v01, v11, fx, fy);
45 | }
46 | 
47 | template<class T>
48 | void write_matlab_array(std::ostream &output, Array2<T, Array1<T> >&a, const char *variable_name, bool transpose=false)
49 | {
50 |    output<<variable_name<<"=[";
51 |    for(int j = 0; j < a.nj; ++j) {
52 |       for(int i = 0; i < a.ni; ++i)  {
53 |          output<<a(i,j)<<" ";
54 |       }
55 |       output<<";";
56 |    }
57 |    output<<"]";
58 |    if(transpose)
59 |       output<<"'";
60 |    output<<";"<<std::endl;
61 | }
62 | 
63 | #endif
64 | 


--------------------------------------------------------------------------------
/fluidsim.cpp:
--------------------------------------------------------------------------------
  1 | #include "fluidsim.h"
  2 | 
  3 | #include "array2_utils.h"
  4 | 
  5 | #include "pcgsolver/sparse_matrix.h"
  6 | #include "pcgsolver/pcg_solver.h"
  7 | 
  8 | float fraction_inside(float phi_left, float phi_right);
  9 | void extrapolate(Array2f& grid, Array2c& valid);
 10 | 
 11 | float circle_phi(const Vec2f& pos) {
 12 |    Vec2f centre(0.5f,0.75f);
 13 |    float rad = 0.1f;
 14 |    Vec2f centre1(0.4f, 0.3f);
 15 |    float rad1 = 0.15f;
 16 |    float phi0 = dist(centre, pos) - rad;
 17 |    float phi1 = dist(centre1, pos) - rad1;
 18 |    return min(phi0,phi1);
 19 | }
 20 | 
 21 | void FluidSim::initialize(float width, int ni_, int nj_) {
 22 |    ni = ni_;
 23 |    nj = nj_;
 24 |    dx = width / (float)ni;
 25 |    u.resize(ni+1,nj); temp_u.resize(ni+1,nj); u_weights.resize(ni+1,nj); u_valid.resize(ni+1,nj); u_vol.resize(ni+1,nj);
 26 |    v.resize(ni,nj+1); temp_v.resize(ni,nj+1); v_weights.resize(ni,nj+1); v_valid.resize(ni,nj+1); v_vol.resize(ni,nj+1);
 27 |    c_vol.resize(ni,nj);
 28 |    n_vol.resize(ni+1,nj+1);
 29 |    u.set_zero();
 30 |    v.set_zero();
 31 |    nodal_solid_phi.resize(ni+1,nj+1);
 32 |    valid.resize(ni+1, nj+1);
 33 |    old_valid.resize(ni+1, nj+1);
 34 |    liquid_phi.resize(ni,nj);
 35 |    particle_radius = dx/sqrt(2.0f);
 36 |    viscosity.resize(ni,nj);
 37 |    viscosity.assign(1.0f);
 38 |    //surface.reset_phi(circle_phi, dx, Vec2f(0.5*dx,0.5*dx), ni, nj);
 39 | }
 40 | 
 41 | //Initialize the grid-based signed distance field that dictates the position of the solid boundary
 42 | void FluidSim::set_boundary(float (*phi)(const Vec2f&)) {
 43 |    
 44 |    for(int j = 0; j < nj+1; ++j) for(int i = 0; i < ni+1; ++i) {
 45 |       Vec2f pos(i*dx,j*dx);
 46 |       nodal_solid_phi(i,j) = phi(pos);
 47 |    }
 48 | 
 49 | }
 50 | 
 51 | float FluidSim::cfl() {
 52 |    float maxvel = 0;
 53 |    for(unsigned int i = 0; i < u.a.size(); ++i)
 54 |       maxvel = max(maxvel, fabs(u.a[i]));
 55 |    for(unsigned int i = 0; i < v.a.size(); ++i)
 56 |       maxvel = max(maxvel, fabs(v.a[i]));
 57 |    return dx / maxvel;
 58 | }
 59 | 
 60 | //The main fluid simulation step
 61 | void FluidSim::advance(float dt) {
 62 |    float t = 0;
 63 |    
 64 |    while(t < dt) {
 65 |       float substep = cfl();   
 66 |       if(t + substep > dt)
 67 |          substep = dt - t;
 68 |    
 69 |       //Passively advect particles
 70 |       advect_particles(substep);
 71 |      
 72 |       //Estimate the liquid signed distance
 73 |       compute_phi();
 74 | 
 75 |       //Advance the velocity
 76 |       advect(substep);
 77 |       add_force(substep);
 78 | 
 79 |       apply_viscosity(substep);
 80 | 
 81 |       apply_projection(substep); 
 82 |       
 83 |       //Pressure projection only produces valid velocities in faces with non-zero associated face area.
 84 |       //Because the advection step may interpolate from these invalid faces, 
 85 |       //we must extrapolate velocities from the fluid domain into these zero-area faces.
 86 |       extrapolate(u, u_valid);
 87 |       extrapolate(v, v_valid);
 88 | 
 89 |       //For extrapolated velocities, replace the normal component with
 90 |       //that of the object.
 91 |       constrain_velocity();
 92 |    
 93 |       t+=substep;
 94 |    }
 95 | }
 96 | 
 97 | void FluidSim::add_force(float dt) {
 98 |    
 99 |    for(int j = 0; j < nj+1; ++j) for(int i = 0; i < ni; ++i) {
100 |       v(i,j) -= 0.1f;
101 |    }
102 |    
103 | }
104 | 
105 | //For extrapolated points, replace the normal component
106 | //of velocity with the object velocity (in this case zero).
107 | void FluidSim::constrain_velocity() {
108 |    temp_u = u;
109 |    temp_v = v;
110 |    
111 |    //(At lower grid resolutions, the normal estimate from the signed
112 |    //distance function is poor, so it doesn't work quite as well.
113 |    //An exact normal would do better.)
114 |    
115 |    //constrain u
116 |    for(int j = 0; j < u.nj; ++j) for(int i = 0; i < u.ni; ++i) {
117 |       if(u_weights(i,j) == 0) {
118 |          //apply constraint
119 |          Vec2f pos(i*dx, (j+0.5f)*dx);
120 |          Vec2f vel = get_velocity(pos);
121 |          Vec2f normal(0,0);
122 |          interpolate_gradient(normal, pos/dx, nodal_solid_phi); 
123 |          normalize(normal);
124 |          float perp_component = dot(vel, normal);
125 |          vel -= perp_component*normal;
126 |          temp_u(i,j) = vel[0];
127 |       }
128 |    }
129 |    
130 |    //constrain v
131 |    for(int j = 0; j < v.nj; ++j) for(int i = 0; i < v.ni; ++i) {
132 |       if(v_weights(i,j) == 0) {
133 |          //apply constraint
134 |          Vec2f pos((i+0.5f)*dx, j*dx);
135 |          Vec2f vel = get_velocity(pos);
136 |          Vec2f normal(0,0);
137 |          interpolate_gradient(normal, pos/dx, nodal_solid_phi); 
138 |          normalize(normal);
139 |          float perp_component = dot(vel, normal);
140 |          vel -= perp_component*normal;
141 |          temp_v(i,j) = vel[1];
142 |       }
143 |    }
144 |    
145 |    //update
146 |    u = temp_u;
147 |    v = temp_v;
148 | 
149 | }
150 | 
151 | //Add a tracer particle for visualization
152 | void FluidSim::add_particle(const Vec2f& position) {
153 |    particles.push_back(position);
154 | }
155 | 
156 | //Basic first order semi-Lagrangian advection of velocities
157 | void FluidSim::advect(float dt) {
158 |    
159 |    //semi-Lagrangian advection on u-component of velocity
160 |    for(int j = 0; j < nj; ++j) for(int i = 0; i < ni+1; ++i) {
161 |       Vec2f pos(i*dx, (j+0.5f)*dx);
162 |       pos = trace_rk2(pos, -dt);
163 |       temp_u(i,j) = get_velocity(pos)[0];  
164 |    }
165 | 
166 |    //semi-Lagrangian advection on v-component of velocity
167 |    for(int j = 0; j < nj+1; ++j) for(int i = 0; i < ni; ++i) {
168 |       Vec2f pos((i+0.5f)*dx, j*dx);
169 |       pos = trace_rk2(pos, -dt);
170 |       temp_v(i,j) = get_velocity(pos)[1];
171 |    }
172 | 
173 |    //move update velocities into u/v vectors
174 |    u = temp_u;
175 |    v = temp_v;
176 | }
177 | 
178 | //Perform 2nd order Runge Kutta to move the particles in the fluid
179 | void FluidSim::advect_particles(float dt) {
180 |    
181 |    for(unsigned int p = 0; p < particles.size(); ++p) {
182 |       Vec2f before = particles[p];
183 |       Vec2f start_velocity = get_velocity(before);
184 |       Vec2f midpoint = before + 0.5f*dt*start_velocity;
185 |       Vec2f mid_velocity = get_velocity(midpoint);
186 |       particles[p] += dt*mid_velocity;
187 |       Vec2f after = particles[p];
188 |       if(dist(before,after) > 3*dx) {
189 |          std::cout << "Before: " << before << " " << "After: " << after << std::endl;
190 |          std::cout << "Mid point: " << midpoint << std::endl;
191 |          std::cout << "Start velocity: " << start_velocity << "  Time step: " << dt << std::endl;
192 |          std::cout << "Mid velocity: " << mid_velocity << std::endl;
193 |       }
194 | 
195 |       //Particles can still occasionally leave the domain due to truncation errors, 
196 |       //interpolation error, or large timesteps, so we project them back in for good measure.
197 |       
198 |       //Try commenting this section out to see the degree of accumulated error.
199 |       float phi_value = interpolate_value(particles[p]/dx, nodal_solid_phi);
200 |       if(phi_value < 0) {
201 |          Vec2f normal;
202 |          interpolate_gradient(normal, particles[p]/dx, nodal_solid_phi);        
203 |          normalize(normal);
204 |          particles[p] -= phi_value*normal;
205 |       }
206 |    }
207 | 
208 | }
209 | 
210 | 
211 | void FluidSim::compute_phi() {
212 |    
213 |    //Estimate from particles
214 |    liquid_phi.assign(3*dx);
215 |    for(unsigned int p = 0; p < particles.size(); ++p) {
216 |       Vec2f point = particles[p];
217 |       int i,j;
218 |       float fx,fy;
219 |       //determine containing cell;
220 |       get_barycentric((point[0])/dx-0.5f, i, fx, 0, ni);
221 |       get_barycentric((point[1])/dx-0.5f, j, fy, 0, nj);
222 |       
223 |       //compute distance to surrounding few points, keep if it's the minimum
224 |       for(int j_off = j-2; j_off<=j+2; ++j_off) for(int i_off = i-2; i_off<=i+2; ++i_off) {
225 |          if(i_off < 0 || i_off >= ni || j_off < 0 || j_off >= nj)
226 |             continue;
227 |          
228 |          Vec2f pos((i_off+0.5f)*dx, (j_off+0.5f)*dx);
229 |          float phi_temp = dist(pos, point) - 1.02f*particle_radius;
230 |          liquid_phi(i_off,j_off) = min(liquid_phi(i_off,j_off), phi_temp);
231 |       }
232 |    }
233 |    
234 |    //"extrapolate" phi into solids if nearby
235 |    for(int j = 0; j < nj; ++j) {
236 |       for(int i = 0; i < ni; ++i) {
237 |          if(liquid_phi(i,j) < 0.5*dx) {
238 |             float solid_phi_val = 0.25f*(nodal_solid_phi(i,j) + nodal_solid_phi(i+1,j) + nodal_solid_phi(i,j+1) + nodal_solid_phi(i+1,j+1));
239 |             if(solid_phi_val < 0)
240 |                liquid_phi(i,j) = -0.5f*dx;
241 |          }
242 |       }
243 |    }  
244 | }
245 | 
246 | 
247 | 
248 | void FluidSim::apply_projection(float dt) {
249 | 
250 |    //Compute finite-volume type face area weight for each velocity sample.
251 |    compute_pressure_weights();
252 |    
253 |    //Set up and solve the variational pressure solve.
254 |    solve_pressure(dt);
255 | 
256 | }
257 | 
258 | void FluidSim::apply_viscosity(float dt) {
259 |    
260 |    printf("Computing weights\n");
261 |    //Estimate weights at velocity and stress positions
262 |    compute_viscosity_weights();
263 | 
264 |    printf("Setting up solve\n");
265 |    //Set up and solve the linear system
266 |    solve_viscosity(dt);
267 | 
268 | }  
269 | 
270 | //Apply RK2 to advect a point in the domain.
271 | Vec2f FluidSim::trace_rk2(const Vec2f& position, float dt) {
272 |    Vec2f input = position;
273 |    Vec2f velocity = get_velocity(input);
274 |    velocity = get_velocity(input + 0.5f*dt*velocity);
275 |    input += dt*velocity;
276 |    return input;
277 | }
278 | 
279 | //Interpolate velocity from the MAC grid.
280 | Vec2f FluidSim::get_velocity(const Vec2f& position) {
281 |    
282 |    //Interpolate the velocity from the u and v grids
283 |    float u_value = interpolate_value(position / dx - Vec2f(0, 0.5f), u);
284 |    float v_value = interpolate_value(position / dx - Vec2f(0.5f, 0), v);
285 |    
286 |    return Vec2f(u_value, v_value);
287 | }
288 | 
289 | 
290 | //Given two signed distance values, determine what fraction of a connecting segment is "inside"
291 | float fraction_inside(float phi_left, float phi_right) {
292 |    if(phi_left < 0 && phi_right < 0)
293 |       return 1;
294 |    if (phi_left < 0 && phi_right >= 0)
295 |       return phi_left / (phi_left - phi_right);
296 |    if(phi_left >= 0 && phi_right < 0)
297 |       return phi_right / (phi_right - phi_left);
298 |    else
299 |       return 0;
300 | }
301 | 
302 | //Compute finite-volume style face-weights for fluid from nodal signed distances
303 | void FluidSim::compute_pressure_weights() {
304 | 
305 |    for(int j = 0; j < u_weights.nj; ++j) for(int i = 0; i < u_weights.ni; ++i) {
306 |       u_weights(i,j) = 1 - fraction_inside(nodal_solid_phi(i,j+1), nodal_solid_phi(i,j));
307 |       u_weights(i,j) = clamp(u_weights(i,j), 0.0f, 1.0f);
308 |    }
309 |    for(int j = 0; j < v_weights.nj; ++j) for(int i = 0; i < v_weights.ni; ++i) {
310 |       v_weights(i,j) = 1 - fraction_inside(nodal_solid_phi(i+1,j), nodal_solid_phi(i,j));
311 |       v_weights(i,j) = clamp(v_weights(i,j), 0.0f, 1.0f);
312 |    }
313 | 
314 | }
315 | 
316 | 
317 | void compute_volume_fractions(const Array2f& levelset, Array2f& fractions, Vec2f fraction_origin, int subdivision) {
318 |    
319 |    //Assumes levelset and fractions have the same dx
320 |    float sub_dx = 1.0f / subdivision;
321 |    int sample_max = subdivision*subdivision;
322 |    for(int j = 0; j < fractions.nj; ++j) {
323 |       for(int i = 0; i < fractions.ni; ++i) {
324 |          float start_x = fraction_origin[0] + (float)i;
325 |          float start_y = fraction_origin[1] + (float)j;
326 |          int incount = 0;
327 | 
328 |          for(int sub_j = 0; sub_j < subdivision; ++sub_j) {
329 |             for(int sub_i = 0; sub_i < subdivision; ++sub_i) {
330 |                float x_pos = start_x + (sub_i+0.5f)*sub_dx;
331 |                float y_pos = start_y + (sub_j+0.5f)*sub_dx;
332 |                float phi_val = interpolate_value(Vec2f(x_pos,y_pos), levelset);
333 |                if(phi_val < 0) 
334 |                   ++incount;
335 |             }
336 |          }
337 |          fractions(i,j) = (float)incount / (float)sample_max;
338 |       }
339 |    }
340 | 
341 | }
342 | 
343 | void FluidSim::compute_viscosity_weights() {
344 |    
345 |    compute_volume_fractions(liquid_phi, c_vol, Vec2f(-0.5,-0.5), 2);
346 |    compute_volume_fractions(liquid_phi, n_vol, Vec2f(-1, -1), 2);
347 |    compute_volume_fractions(liquid_phi, u_vol, Vec2f(-1,-0.5), 2);
348 |    compute_volume_fractions(liquid_phi, v_vol, Vec2f(-0.5,-1), 2);
349 | 
350 | }
351 | 
352 | //An implementation of the variational pressure projection solve for static geometry
353 | void FluidSim::solve_pressure(float dt) {
354 |    
355 |    //This linear system could be simplified, but I've left it as is for clarity 
356 |    //and consistency with the standard naive discretization
357 |    
358 |    int ni = v.ni;
359 |    int nj = u.nj;
360 |    int system_size = ni*nj;
361 |    if(rhs.size() != system_size) {
362 |       rhs.resize(system_size);
363 |       pressure.resize(system_size);
364 |       matrix.resize(system_size);
365 |    }
366 |    matrix.zero();
367 |    
368 |    //Build the linear system for pressure
369 |    for(int j = 1; j < nj-1; ++j) {
370 |       for(int i = 1; i < ni-1; ++i) {
371 |          int index = i + ni*j;
372 |          rhs[index] = 0;
373 |          pressure[index] = 0;
374 |          float centre_phi = liquid_phi(i,j);
375 |          if(centre_phi < 0) {
376 | 
377 |             //right neighbour
378 |             float term = u_weights(i+1,j) * dt / sqr(dx);
379 |             float right_phi = liquid_phi(i+1,j);
380 |             if(right_phi < 0) {
381 |                matrix.add_to_element(index, index, term);
382 |                matrix.add_to_element(index, index + 1, -term);
383 |             }
384 |             else {
385 |                float theta = fraction_inside(centre_phi, right_phi);
386 |                if(theta < 0.01f) theta = 0.01f;
387 |                matrix.add_to_element(index, index, term/theta);
388 |             }
389 |             rhs[index] -= u_weights(i+1,j)*u(i+1,j) / dx;
390 |             
391 |             //left neighbour
392 |             term = u_weights(i,j) * dt / sqr(dx);
393 |             float left_phi = liquid_phi(i-1,j);
394 |             if(left_phi < 0) {
395 |                matrix.add_to_element(index, index, term);
396 |                matrix.add_to_element(index, index - 1, -term);
397 |             }
398 |             else {
399 |                float theta = fraction_inside(centre_phi, left_phi);
400 |                if(theta < 0.01f) theta = 0.01f;
401 |                matrix.add_to_element(index, index, term/theta);
402 |             }
403 |             rhs[index] += u_weights(i,j)*u(i,j) / dx;
404 |             
405 |             //top neighbour
406 |             term = v_weights(i,j+1) * dt / sqr(dx);
407 |             float top_phi = liquid_phi(i,j+1);
408 |             if(top_phi < 0) {
409 |                matrix.add_to_element(index, index, term);
410 |                matrix.add_to_element(index, index + ni, -term);
411 |             }
412 |             else {
413 |                float theta = fraction_inside(centre_phi, top_phi);
414 |                if(theta < 0.01f) theta = 0.01f;
415 |                matrix.add_to_element(index, index, term/theta);
416 |             }
417 |             rhs[index] -= v_weights(i,j+1)*v(i,j+1) / dx;
418 |             
419 |             //bottom neighbour
420 |             term = v_weights(i,j) * dt / sqr(dx);
421 |             float bot_phi = liquid_phi(i,j-1);
422 |             if(bot_phi < 0) {
423 |                matrix.add_to_element(index, index, term);
424 |                matrix.add_to_element(index, index - ni, -term);
425 |             }
426 |             else {
427 |                float theta = fraction_inside(centre_phi, bot_phi);
428 |                if(theta < 0.01f) theta = 0.01f;
429 |                matrix.add_to_element(index, index, term/theta);
430 |             }
431 |             rhs[index] += v_weights(i,j)*v(i,j) / dx;
432 |          }
433 |       }
434 |    }
435 | 
436 |    //Solve the system using Robert Bridson's incomplete Cholesky PCG solver
437 |    
438 |    double tolerance;
439 |    int iterations;
440 |    bool success = solver.solve(matrix, rhs, pressure, tolerance, iterations);
441 |    if(!success) {
442 |       printf("WARNING: Pressure solve failed!************************************************\n");
443 |    }
444 |    
445 |    //Apply the velocity update
446 |    u_valid.assign(0);
447 |    for(int j = 0; j < u.nj; ++j) for(int i = 1; i < u.ni-1; ++i) {
448 |       int index = i + j*ni;
449 |       if(u_weights(i,j) > 0 && (liquid_phi(i,j) < 0 || liquid_phi(i-1,j) < 0)) {
450 |          float theta = 1;
451 |          if(liquid_phi(i,j) >= 0 || liquid_phi(i-1,j) >= 0)
452 |             theta = fraction_inside(liquid_phi(i-1,j), liquid_phi(i,j));
453 |          if(theta < 0.01f) theta = 0.01f;
454 |          u(i,j) -= dt  * (float)(pressure[index] - pressure[index-1]) / dx / theta; 
455 |          u_valid(i,j) = 1;
456 |       }
457 |       else
458 |          u(i,j) = 0;
459 |    }
460 |    v_valid.assign(0);
461 |    for(int j = 1; j < v.nj-1; ++j) for(int i = 0; i < v.ni; ++i) {
462 |       int index = i + j*ni;
463 |       if(v_weights(i,j) > 0 && (liquid_phi(i,j) < 0 || liquid_phi(i,j-1) < 0)) {
464 |          float theta = 1;
465 |          if(liquid_phi(i,j) >= 0 || liquid_phi(i,j-1) >= 0)
466 |             theta = fraction_inside(liquid_phi(i,j-1), liquid_phi(i,j));
467 |          if(theta < 0.01f) theta = 0.01f;
468 |          v(i,j) -= dt  * (float)(pressure[index] - pressure[index-ni]) / dx / theta; 
469 |          v_valid(i,j) = 1;
470 |       }
471 |       else
472 |          v(i,j) = 0;
473 |    }
474 | 
475 | }
476 | 
477 | int FluidSim::u_ind(int i, int j) {
478 |    return i + j*(ni+1);
479 | }
480 | 
481 | int FluidSim::v_ind(int i, int j) {
482 |    return i + j*ni + (ni+1)*nj;
483 | }
484 | 
485 | 
486 | void FluidSim::solve_viscosity(float dt) {
487 |    int ni = liquid_phi.ni;
488 |    int nj = liquid_phi.nj;
489 |    
490 |    //static obstacles for simplicity - for moving objects, 
491 |    //use a spatially varying 2d array, and modify the linear system appropriately
492 |    float u_obj = 0;
493 |    float v_obj = 0;
494 | 
495 |    Array2c u_state(ni+1,nj,(const char&)0);
496 |    Array2c v_state(ni,nj+1,(const char&)0);
497 |    const int SOLID = 1;
498 |    const int FLUID = 0;
499 | 
500 |    printf("Determining states\n");
501 |    //just determine if the face position is inside the wall! That's it.
502 |    for(int j = 0; j < nj; ++j) {
503 |       for(int i = 0; i < ni+1; ++i) {
504 |          if(i - 1 < 0 || i >= ni || (nodal_solid_phi(i,j+1) + nodal_solid_phi(i,j))/2 <= 0)
505 |             u_state(i,j) = SOLID;
506 |          else
507 |             u_state(i,j) = FLUID;
508 |       }
509 |    }
510 | 
511 | 
512 |    for(int j = 0; j < nj+1; ++j)  {
513 |       for(int i = 0; i < ni; ++i) {
514 |          if(j - 1 < 0 || j >= nj || (nodal_solid_phi(i+1,j) + nodal_solid_phi(i,j))/2 <= 0)
515 |             v_state(i,j) = SOLID;
516 |          else 
517 |             v_state(i,j) = FLUID;
518 |       }
519 |    }
520 |    
521 |    printf("Building matrix\n");
522 |    int elts = (ni+1)*nj + ni*(nj+1);
523 |    if(vrhs.size() != elts) {
524 |       vrhs.resize(elts);
525 |       velocities.resize(elts);
526 |       vmatrix.resize(elts);
527 |    }
528 |    vmatrix.zero();
529 |    
530 |    float factor = dt/sqr(dx);
531 |    for(int j = 1; j < nj-1; ++j) for(int i = 1; i < ni-1; ++i) {
532 |       if(u_state(i,j) == FLUID ) {
533 |          int index = u_ind(i,j);      
534 |          
535 |          vrhs[index] = u_vol(i,j) * u(i,j);
536 |          vmatrix.set_element(index,index,u_vol(i,j));
537 |          
538 |          //uxx terms
539 |          float visc_right = viscosity(i,j);
540 |          float visc_left = viscosity(i-1,j);
541 |          float vol_right = c_vol(i,j);
542 |          float vol_left = c_vol(i-1,j);
543 | 
544 |         //u_x_right
545 |          vmatrix.add_to_element(index,index, 2*factor*visc_right*vol_right);
546 |          if(u_state(i+1,j) == FLUID)
547 |             vmatrix.add_to_element(index,u_ind(i+1,j), -2*factor*visc_right*vol_right);
548 |          else if(u_state(i+1,j) == SOLID)
549 |             vrhs[index] -= -2*factor*visc_right*vol_right*u_obj;
550 | 
551 |          //u_x_left
552 |          vmatrix.add_to_element(index,index, 2*factor*visc_left*vol_left);
553 |          if(u_state(i-1,j) == FLUID)
554 |             vmatrix.add_to_element(index,u_ind(i-1,j), -2*factor*visc_left*vol_left);
555 |          else if(u_state(i-1,j) == SOLID)
556 |             vrhs[index] -= -2*factor*visc_left*vol_left*u_obj;
557 |          
558 |          //uyy terms
559 |          float visc_top = 0.25f*(viscosity(i-1,j+1) + viscosity(i-1,j) + viscosity(i,j+1) + viscosity(i,j));
560 |          float visc_bottom = 0.25f*(viscosity(i-1,j) + viscosity(i-1,j-1) + viscosity(i,j) + viscosity(i,j-1));
561 |          float vol_top = n_vol(i,j+1);
562 |          float vol_bottom = n_vol(i,j);
563 | 
564 |          //u_y_top
565 |          vmatrix.add_to_element(index,index, +factor*visc_top*vol_top);
566 |          if(u_state(i,j+1) == FLUID)
567 |             vmatrix.add_to_element(index,u_ind(i,j+1), -factor*visc_top*vol_top);
568 |          else if(u_state(i,j+1) == SOLID)
569 |             vrhs[index] -= -u_obj*factor*visc_top*vol_top;
570 |       
571 |          //u_y_bottom
572 |          vmatrix.add_to_element(index,index, +factor*visc_bottom*vol_bottom);
573 |          if(u_state(i,j-1) == FLUID)
574 |             vmatrix.add_to_element(index,u_ind(i,j-1), -factor*visc_bottom*vol_bottom);
575 |          else if(u_state(i,j-1) == SOLID)
576 |             vrhs[index] -= -u_obj*factor*visc_bottom*vol_bottom;
577 |       
578 |          //vxy terms
579 |          //v_x_top
580 |          if(v_state(i,j+1) == FLUID)
581 |             vmatrix.add_to_element(index,v_ind(i,j+1), -factor*visc_top*vol_top);
582 |          else if(v_state(i,j+1) == SOLID)
583 |             vrhs[index] -= -v_obj*factor*visc_top*vol_top;
584 |          
585 |          if(v_state(i-1,j+1) == FLUID)
586 |             vmatrix.add_to_element(index,v_ind(i-1,j+1), factor*visc_top*vol_top);
587 |          else if(v_state(i-1,j+1) == SOLID)
588 |             vrhs[index] -= v_obj*factor*visc_top*vol_top;
589 |      
590 |          //v_x_bottom
591 |          if(v_state(i,j) == FLUID)
592 |             vmatrix.add_to_element(index,v_ind(i,j), +factor*visc_bottom*vol_bottom);
593 |          else if(v_state(i,j) == SOLID)
594 |             vrhs[index] -= v_obj*factor*visc_bottom*vol_bottom;
595 |          
596 |          if(v_state(i-1,j) == FLUID)
597 |             vmatrix.add_to_element(index,v_ind(i-1,j), -factor*visc_bottom*vol_bottom);
598 |          else if(v_state(i-1,j) == SOLID)
599 |             vrhs[index] -= -v_obj*factor*visc_bottom*vol_bottom;
600 |       
601 | 
602 |       }
603 |    }
604 |    
605 |    for(int j = 1; j < nj; ++j) for(int i = 1; i < ni-1; ++i) {
606 |       if(v_state(i,j) == FLUID) {
607 |          int index = v_ind(i,j);      
608 |          
609 |          vrhs[index] = v_vol(i,j)*v(i,j);
610 |          vmatrix.set_element(index, index, v_vol(i,j));
611 | 
612 |          //vyy
613 |          float visc_top = viscosity(i,j);
614 |          float visc_bottom = viscosity(i,j-1);
615 |          float vol_top = c_vol(i,j);
616 |          float vol_bottom = c_vol(i,j-1);
617 | 
618 |          //vy_top
619 |          vmatrix.add_to_element(index,index, +2*factor*visc_top*vol_top);
620 |          if(v_state(i,j+1) == FLUID)
621 |             vmatrix.add_to_element(index,v_ind(i,j+1), -2*factor*visc_top*vol_top);
622 |          else if (v_state(i,j+1) == SOLID)
623 |             vrhs[index] -= -2*factor*visc_top*vol_top*v_obj;
624 |          
625 |          //vy_bottom
626 |          vmatrix.add_to_element(index,index, +2*factor*visc_bottom*vol_bottom);
627 |          if(v_state(i,j-1) == FLUID)
628 |             vmatrix.add_to_element(index,v_ind(i,j-1), -2*factor*visc_bottom*vol_bottom);
629 |          else if(v_state(i,j-1) == SOLID)
630 |             vrhs[index] -= -2*factor*visc_bottom*vol_bottom*v_obj;
631 |          
632 |          //vxx terms
633 |          float visc_right = 0.25f*(viscosity(i,j-1) + viscosity(i+1,j-1) + viscosity(i,j) + viscosity(i+1,j));
634 |          float visc_left = 0.25f*(viscosity(i,j-1) + viscosity(i-1,j-1) + viscosity(i,j) + viscosity(i-1,j));
635 |          float vol_right = n_vol(i+1,j);
636 |          float vol_left = n_vol(i,j);
637 | 
638 |          //v_x_right
639 |          vmatrix.add_to_element(index,index, +factor*visc_right*vol_right);
640 |          if(v_state(i+1,j) == FLUID)
641 |             vmatrix.add_to_element(index,v_ind(i+1,j), -factor*visc_right*vol_right);
642 |          else if(v_state(i+1,j) == SOLID)
643 |             vrhs[index] -= -v_obj*factor*visc_right*vol_right;
644 |       
645 |          //v_x_left
646 |          vmatrix.add_to_element(index,index, +factor*visc_left*vol_left);
647 |          if(v_state(i-1,j) == FLUID)
648 |             vmatrix.add_to_element(index,v_ind(i-1,j), -factor*visc_left*vol_left);
649 |          else if(v_state(i-1,j) == SOLID)
650 |             vrhs[index] -= -v_obj*factor*visc_left*vol_left;
651 | 
652 |          //uyx
653 | 
654 |          //u_y_right
655 |          if(u_state(i+1,j) == FLUID)
656 |             vmatrix.add_to_element(index,u_ind(i+1,j), -factor*visc_right*vol_right);
657 |          else if(u_state(i+1,j) == SOLID)
658 |             vrhs[index] -= -u_obj*factor*visc_right*vol_right;
659 |          
660 |          if(u_state(i+1,j-1) == FLUID)
661 |             vmatrix.add_to_element(index,u_ind(i+1,j-1), factor*visc_right*vol_right);
662 |          else if(u_state(i+1,j-1) == SOLID)
663 |             vrhs[index] -= u_obj*factor*visc_right*vol_right;
664 |       
665 |          //u_y_left
666 |          if(u_state(i,j) == FLUID)
667 |             vmatrix.add_to_element(index,u_ind(i,j), factor*visc_left*vol_left);
668 |          else if(u_state(i,j) == SOLID)
669 |             vrhs[index] -= u_obj*factor*visc_left*vol_left;
670 |           
671 |          if(u_state(i,j-1) == FLUID)
672 |             vmatrix.add_to_element(index,u_ind(i,j-1), -factor*visc_left*vol_left);
673 |          else if(u_state(i,j-1) == SOLID)
674 |             vrhs[index] -= -u_obj*factor*visc_left*vol_left;
675 |       
676 |       }
677 |    }
678 | 
679 |    
680 |    double res_out;
681 |    int iter_out;
682 |    
683 |    solver.solve(vmatrix, vrhs, velocities, res_out, iter_out);
684 |    
685 |    for(int j = 0; j < nj; ++j)
686 |       for(int i = 0; i < ni+1; ++i)
687 |          if(u_state(i,j) == FLUID)
688 |             u(i,j) = (float)velocities[u_ind(i,j)];
689 |          else if(u_state(i,j) == SOLID) 
690 |             u(i,j) = u_obj;
691 |          
692 | 
693 |    
694 |    for(int j = 0; j < nj+1; ++j)
695 |       for(int i = 0; i < ni; ++i)
696 |          if(v_state(i,j) == FLUID)
697 |             v(i,j) = (float)velocities[v_ind(i,j)];
698 |          else if(v_state(i,j) == SOLID) 
699 |             v(i,j) = v_obj;
700 | }
701 | 
702 | 
703 | 
704 | 
705 | //Apply several iterations of a very simple "Jacobi"-style propagation of valid velocity data in all directions
706 | void extrapolate(Array2f& grid, Array2c& valid) {
707 |    
708 |    Array2c old_valid(valid.ni,valid.nj);
709 |    for(int layers = 0; layers < 10; ++layers) {
710 |       old_valid = valid;
711 |       Array2f temp_grid = grid;
712 |       for(int j = 1; j < grid.nj-1; ++j) for(int i = 1; i < grid.ni-1; ++i) {
713 |          float sum = 0;
714 |          int count = 0;
715 |          
716 |          if(!old_valid(i,j)) {
717 |             
718 |             if(old_valid(i+1,j)) {
719 |                sum += grid(i+1,j);\
720 |                ++count;
721 |             }
722 |             if(old_valid(i-1,j)) {
723 |                sum += grid(i-1,j);\
724 |                ++count;
725 |             }
726 |             if(old_valid(i,j+1)) {
727 |                sum += grid(i,j+1);\
728 |                ++count;
729 |             }
730 |             if(old_valid(i,j-1)) {
731 |                sum += grid(i,j-1);\
732 |                ++count;
733 |             }
734 | 
735 |             //If any of neighbour cells were valid, 
736 |             //assign the cell their average value and tag it as valid
737 |             if(count > 0) {
738 |                temp_grid(i,j) = sum /(float)count;
739 |                valid(i,j) = 1;
740 |             }
741 | 
742 |          }
743 |       }
744 |       grid = temp_grid;
745 | 
746 |    }
747 | 
748 | }
749 | 


--------------------------------------------------------------------------------
/fluidsim.h:
--------------------------------------------------------------------------------
 1 | #ifndef FLUIDSIM_H
 2 | #define FLUIDSIM_H
 3 | 
 4 | #include "array2.h"
 5 | #include "vec.h"
 6 | #include "pcgsolver/sparse_matrix.h"
 7 | #include "pcgsolver/pcg_solver.h"
 8 | 
 9 | #include <vector>
10 | 
11 | class FluidSim {
12 | 
13 | public:
14 |    void initialize(float width, int ni_, int nj_);
15 |    void set_boundary(float (*phi)(const Vec2f&));
16 |    void advance(float dt);
17 | 
18 |    //Grid dimensions
19 |    int ni,nj;
20 |    float dx;
21 |    
22 |    //Fluid velocity
23 |    Array2f u, v;
24 |    Array2f temp_u, temp_v;
25 |    
26 |    //Static geometry representation
27 |    Array2f nodal_solid_phi;
28 |    
29 |    //Data for pressure solve and extrapolation
30 |    Array2c u_valid, v_valid;
31 |    Array2f liquid_phi; //extracted from particles
32 |    Array2f u_weights, v_weights; 
33 | 
34 |    //Data for viscosity solve
35 |    Array2f u_vol, v_vol, c_vol, n_vol;
36 |    Array2f viscosity;
37 | 
38 |    std::vector<Vec2f> particles; //For marker particle simulation
39 |    float particle_radius;
40 |    
41 |    //Data arrays for extrapolation
42 |    Array2c valid, old_valid;
43 | 
44 |    //Solver data
45 |    PCGSolver<double> solver;
46 |    SparseMatrixd matrix;
47 |    std::vector<double> rhs;
48 |    std::vector<double> pressure;
49 | 
50 |    SparseMatrixd vmatrix;
51 |    std::vector<double> vrhs;
52 |    std::vector<double> velocities;
53 | 
54 |    Vec2f get_velocity(const Vec2f& position);
55 |    void add_particle(const Vec2f& position);
56 | 
57 | private:
58 | 
59 |    Vec2f trace_rk2(const Vec2f& position, float dt);
60 | 
61 |    void advect_particles(float dt);
62 | 
63 |    void compute_phi();
64 | 
65 |    float cfl();
66 | 
67 |    //fluid velocity operations
68 |    void advect(float dt);
69 |    void add_force(float dt);
70 | 
71 |    void apply_projection(float dt);
72 |    void compute_pressure_weights();
73 |    void solve_pressure(float dt);
74 |    
75 |    int u_ind(int i, int j);
76 |    int v_ind(int i, int j);
77 |    void apply_viscosity(float dt);
78 |    void compute_viscosity_weights();
79 |    void solve_viscosity(float dt);
80 | 
81 |    void constrain_velocity();
82 | 
83 | };
84 | 
85 | #endif


--------------------------------------------------------------------------------
/gluvi.cpp:
--------------------------------------------------------------------------------
   1 | #include <cmath>
   2 | #include <cstdarg>
   3 | #include <cstdlib>
   4 | #include <fstream>
   5 | #include "gluvi.h"
   6 | #include "vec.h"
   7 | 
   8 | #ifndef M_PI
   9 | #define M_PI 3.14159265358979323846f
  10 | #endif
  11 | 
  12 | using namespace std;
  13 | 
  14 | namespace Gluvi{
  15 | 
  16 | Target3D::
  17 | Target3D(float target_[3], float dist_, float heading_, float pitch_, float fovy_, float near_clip_factor_, float far_clip_factor_)
  18 |    : dist(dist_), heading(heading_), pitch(pitch_), fovy(fovy_), 
  19 |      near_clip_factor(near_clip_factor_), far_clip_factor(far_clip_factor_), action_mode(INACTIVE)
  20 | {
  21 |    if(target_){
  22 |       target[0]=target_[0];
  23 |       target[1]=target_[1];
  24 |       target[2]=target_[2];
  25 |    }else{
  26 |       target[0]=0;
  27 |       target[1]=0;
  28 |       target[2]=0;
  29 |    }
  30 |    default_target[0]=target[0];
  31 |    default_target[1]=target[1];
  32 |    default_target[2]=target[2];
  33 |    default_dist=dist;
  34 |    default_heading=heading;
  35 |    default_pitch=pitch;
  36 | }
  37 | 
  38 | void Target3D::
  39 | click(int button, int state, int x, int y)
  40 | {
  41 |    if(state==GLUT_UP)
  42 |       action_mode=INACTIVE;
  43 |    else if(button==GLUT_LEFT_BUTTON)
  44 |       action_mode=ROTATE;
  45 |    else if(button==GLUT_MIDDLE_BUTTON)
  46 |       action_mode=TRUCK;
  47 |    else if(button==GLUT_RIGHT_BUTTON)
  48 |       action_mode=DOLLY;
  49 |    oldmousex=x;
  50 |    oldmousey=y;
  51 | }
  52 | 
  53 | void Target3D::
  54 | drag(int x, int y)
  55 | {
  56 |    switch(action_mode){
  57 |       case INACTIVE:
  58 |          return; // nothing to do
  59 |       case ROTATE:
  60 |          heading+=0.007*(oldmousex-x);
  61 |          if(heading<-M_PI) heading+=2*M_PI;
  62 |          else if(heading>M_PI) heading-=2*M_PI;
  63 |          pitch+=0.007*(oldmousey-y);
  64 |          if(pitch<-0.5*M_PI) pitch=-0.5*M_PI;
  65 |          else if(pitch>0.5*M_PI) pitch=0.5*M_PI;
  66 |          break;
  67 |       case TRUCK:
  68 |          target[0]+=(0.002*dist)*cos(heading)*(oldmousex-x);
  69 |          target[1]-=(0.002*dist)*(oldmousey-y);
  70 |          target[2]-=(0.002*dist)*sin(heading)*(oldmousex-x);
  71 |          break;
  72 |       case DOLLY:
  73 |          dist*=pow(1.01, oldmousey-y + x-oldmousex);
  74 |          break;
  75 |    }
  76 |    oldmousex=x;
  77 |    oldmousey=y;
  78 |    glutPostRedisplay();
  79 | }
  80 | 
  81 | void Target3D::
  82 | return_to_default(void)
  83 | {
  84 |    target[0]=default_target[0];
  85 |    target[1]=default_target[1];
  86 |    target[2]=default_target[2];
  87 |    dist=default_dist;
  88 |    heading=default_heading;
  89 |    pitch=default_pitch;
  90 | }
  91 | 
  92 | void Target3D::
  93 | transform_mouse(int x, int y, float ray_origin[3], float ray_direction[3])
  94 | {
  95 |    float ch=cos(heading), sh=sin(heading);
  96 |    float cp=cos(pitch), sp=sin(pitch);
  97 | 
  98 |    ray_origin[0]=target[0]+dist*sh*cp;
  99 |    ray_origin[1]=target[1]-dist*sp;
 100 |    ray_origin[2]=target[2]+dist*ch*cp;
 101 | 
 102 |    float scale=0.5*tan(fovy)/winheight;
 103 |    float camx=(x-0.5*winwidth)*scale, camy=(0.5*winheight-y)*scale, camz=-1.0; // in camera coordinates, this is ray_direction (but not normalized)
 104 |    // now need to rotate into world space from camera space
 105 |    float px=camx, py=camy*cp-camz*sp, pz=camy*sp+camz*cp;
 106 |    ray_direction[0]=px*ch+pz*sh;
 107 |    ray_direction[1]=py;
 108 |    ray_direction[2]=-px*sh+pz*ch;
 109 | 
 110 |    //@@@ is this right to get us to the near clipping plane?
 111 |    ray_origin[0]+=near_clip_factor*dist*ray_direction[0];
 112 |    ray_origin[1]+=near_clip_factor*dist*ray_direction[1];
 113 |    ray_origin[2]+=near_clip_factor*dist*ray_direction[2];
 114 | 
 115 |    // normalize direction vector
 116 |    float mag=sqrt(ray_direction[0]*ray_direction[0]
 117 |                   + ray_direction[1]*ray_direction[1]
 118 |                   + ray_direction[2]*ray_direction[2]);
 119 |    ray_direction[0]/=mag;
 120 |    ray_direction[1]/=mag;
 121 |    ray_direction[2]/=mag;
 122 | }
 123 | 
 124 | void Target3D::
 125 | get_viewing_direction(float direction[3])
 126 | {
 127 |    float ch=cos(heading), sh=sin(heading);
 128 |    float cp=cos(pitch), sp=sin(pitch);
 129 |    direction[0]=-sh*cp;
 130 |    direction[1]=sp;
 131 |    direction[2]=-ch*cp;
 132 | }
 133 | 
 134 | void Target3D::
 135 | gl_transform(void)
 136 | {
 137 |    glViewport(0, 0, (GLsizei)winwidth, (GLsizei)winheight);
 138 | 
 139 |    glMatrixMode(GL_PROJECTION);
 140 |    glLoadIdentity();
 141 |    gluPerspective(fovy, winwidth/(float)winheight, near_clip_factor*dist, far_clip_factor*dist);
 142 | 
 143 |    glMatrixMode(GL_MODELVIEW);
 144 |    glLoadIdentity();
 145 |    GLfloat pos[3];
 146 |    pos[0]=target[0]-dist*sin(heading)*cos(pitch);
 147 |    pos[1]=target[1]-dist*sin(pitch);
 148 |    pos[2]=target[2]-dist*cos(heading)*cos(pitch);
 149 |    glTranslatef(0, 0, -dist); // translate target dist away in the z direction
 150 |    glRotatef(-180/M_PI*pitch, 1, 0, 0); // rotate pitch in the yz plane
 151 |    glRotatef(-180/M_PI*heading, 0, 1, 0); // rotate heading in the xz plane
 152 |    glTranslatef(-target[0], -target[1], -target[2]); // translate target to origin
 153 | }
 154 | 
 155 | void Target3D::
 156 | export_rib(ostream &output)
 157 | {
 158 |    output<<"Clipping "<<near_clip_factor*dist<<" "<<far_clip_factor*dist<<endl; // could be more generous here!
 159 |    output<<"Projection \"perspective\" \"fov\" "<<fovy<<endl;
 160 |    output<<"ReverseOrientation"<<endl;  // RenderMan has a different handedness from OpenGL's default
 161 |    output<<"Scale 1 1 -1"<<endl;        // so we need to correct for that here
 162 |    output<<"Translate 0 0 "<<-dist<<endl;
 163 |    output<<"Rotate "<<-180/M_PI*pitch<<" 1 0 0"<<endl;
 164 |    output<<"Rotate "<<-180/M_PI*heading<<" 0 1 0"<<endl;
 165 |    output<<"Translate "<<-target[0]<<" "<<-target[1]<<" "<<-target[2]<<endl;
 166 | }
 167 | 
 168 | //=================================================================================
 169 | 
 170 | TargetOrtho3D::
 171 | TargetOrtho3D(float target_[3], float dist_, float heading_, float pitch_, float height_factor_, float near_clip_factor_, float far_clip_factor_)
 172 |    : dist(dist_), heading(heading_), pitch(pitch_), height_factor(height_factor_), 
 173 |      near_clip_factor(near_clip_factor_), far_clip_factor(far_clip_factor_), action_mode(INACTIVE)
 174 | {
 175 |    if(target_){
 176 |       target[0]=target_[0];
 177 |       target[1]=target_[1];
 178 |       target[2]=target_[2];
 179 |    }else{
 180 |       target[0]=0;
 181 |       target[1]=0;
 182 |       target[2]=0;
 183 |    }
 184 |    default_target[0]=target[0];
 185 |    default_target[1]=target[1];
 186 |    default_target[2]=target[2];
 187 |    default_dist=dist;
 188 |    default_heading=heading;
 189 |    default_pitch=pitch;
 190 | }
 191 | 
 192 | void TargetOrtho3D::
 193 | click(int button, int state, int x, int y)
 194 | {
 195 |    if(state==GLUT_UP)
 196 |       action_mode=INACTIVE;
 197 |    else if(button==GLUT_LEFT_BUTTON)
 198 |       action_mode=ROTATE;
 199 |    else if(button==GLUT_MIDDLE_BUTTON)
 200 |       action_mode=TRUCK;
 201 |    else if(button==GLUT_RIGHT_BUTTON)
 202 |       action_mode=DOLLY;
 203 |    oldmousex=x;
 204 |    oldmousey=y;
 205 | }
 206 | 
 207 | void TargetOrtho3D::
 208 | drag(int x, int y)
 209 | {
 210 |    switch(action_mode){
 211 |       case INACTIVE:
 212 |          return; // nothing to do
 213 |       case ROTATE:
 214 |          heading+=0.007*(oldmousex-x);
 215 |          if(heading<-M_PI) heading+=2*M_PI;
 216 |          else if(heading>M_PI) heading-=2*M_PI;
 217 |          pitch+=0.007*(oldmousey-y);
 218 |          if(pitch<-0.5*M_PI) pitch=-0.5*M_PI;
 219 |          else if(pitch>0.5*M_PI) pitch=0.5*M_PI;
 220 |          break;
 221 |       case TRUCK:
 222 |          target[0]+=(0.002*dist)*cos(heading)*(oldmousex-x);
 223 |          target[1]-=(0.002*dist)*(oldmousey-y);
 224 |          target[2]-=(0.002*dist)*sin(heading)*(oldmousex-x);
 225 |          break;
 226 |       case DOLLY:
 227 |          dist*=pow(1.01, oldmousey-y + x-oldmousex);
 228 |          break;
 229 |    }
 230 |    oldmousex=x;
 231 |    oldmousey=y;
 232 |    glutPostRedisplay();
 233 | }
 234 | 
 235 | void TargetOrtho3D::
 236 | return_to_default(void)
 237 | {
 238 |    target[0]=default_target[0];
 239 |    target[1]=default_target[1];
 240 |    target[2]=default_target[2];
 241 |    dist=default_dist;
 242 |    heading=default_heading;
 243 |    pitch=default_pitch;
 244 | }
 245 | 
 246 | void TargetOrtho3D::
 247 | transform_mouse(int x, int y, float ray_origin[3], float ray_direction[3])
 248 | {
 249 |    // @@@ unimplemented
 250 | }
 251 | 
 252 | void TargetOrtho3D::
 253 | get_viewing_direction(float direction[3])
 254 | {
 255 |    float ch=cos(heading), sh=sin(heading);
 256 |    float cp=cos(pitch), sp=sin(pitch);
 257 |    direction[0]=-sh*cp;
 258 |    direction[1]=sp;
 259 |    direction[2]=-ch*cp;
 260 | }
 261 | 
 262 | void TargetOrtho3D::
 263 | gl_transform(void)
 264 | {
 265 |    glViewport(0, 0, (GLsizei)winwidth, (GLsizei)winheight);
 266 | 
 267 |    glMatrixMode(GL_PROJECTION);
 268 |    glLoadIdentity();
 269 |    float halfheight=0.5*height_factor*dist, halfwidth=halfheight*winwidth/(float)winheight;
 270 |    glOrtho(-halfwidth, halfwidth, -halfheight, halfheight, near_clip_factor*dist, far_clip_factor*dist);
 271 | 
 272 |    glMatrixMode(GL_MODELVIEW);
 273 |    glLoadIdentity();
 274 |    GLfloat pos[3];
 275 |    pos[0]=target[0]-dist*sin(heading)*cos(pitch);
 276 |    pos[1]=target[1]-dist*sin(pitch);
 277 |    pos[2]=target[2]-dist*cos(heading)*cos(pitch);
 278 |    glTranslatef(0, 0, -dist); // translate target dist away in the z direction
 279 |    glRotatef(-180/M_PI*pitch, 1, 0, 0); // rotate pitch in the yz plane
 280 |    glRotatef(-180/M_PI*heading, 0, 1, 0); // rotate heading in the xz plane
 281 |    glTranslatef(-target[0], -target[1], -target[2]); // translate target to origin
 282 | }
 283 | 
 284 | void TargetOrtho3D::
 285 | export_rib(ostream &output)
 286 | {
 287 |    output<<"Clipping "<<near_clip_factor*dist<<" "<<far_clip_factor*dist<<endl; // could be more generous here!
 288 |    output<<"Projection \"orthographic\""<<endl;
 289 |    //@@@ incomplete: need a scaling according to height_factor*dist somewhere in here
 290 |    output<<"ReverseOrientation"<<endl;  // RenderMan has a different handedness from OpenGL's default
 291 |    output<<"Scale 1 1 -1"<<endl;        // so we need to correct for that here
 292 |    output<<"Translate 0 0 "<<-dist<<endl;
 293 |    output<<"Rotate "<<-180/M_PI*pitch<<" 1 0 0"<<endl;
 294 |    output<<"Rotate "<<-180/M_PI*heading<<" 0 1 0"<<endl;
 295 |    output<<"Translate "<<-target[0]<<" "<<-target[1]<<" "<<-target[2]<<endl;
 296 | }
 297 | 
 298 | //=================================================================================
 299 | 
 300 | PanZoom2D::
 301 | PanZoom2D(float bottom_, float left_, float height_)
 302 |    : bottom(bottom_), left(left_), height(height_), action_mode(INACTIVE)
 303 | {
 304 |    default_bottom=bottom;
 305 |    default_left=left;
 306 |    default_height=height;
 307 | }
 308 | 
 309 | void PanZoom2D::
 310 | click(int button, int state, int x, int y)
 311 | {
 312 |    if(state==GLUT_UP){
 313 |       float r=height/winheight;
 314 |       switch(action_mode){
 315 |          case PAN:
 316 |             if(!moved_since_mouse_down){
 317 |                // make mouse click the centre of the window
 318 |                left+=r*(x-0.5*winwidth);
 319 |                bottom+=r*(0.5*winheight-y);
 320 |                glutPostRedisplay();
 321 |             }
 322 |             break;
 323 |          case ZOOM_IN:
 324 |             if(moved_since_mouse_down){
 325 |                // zoom in to selection
 326 |                float desired_width=fabs((x-clickx)*height/winheight);
 327 |                float desired_height=fabs((y-clicky)*height/winheight);
 328 |                if(desired_height==0) desired_height=height/winheight;
 329 |                if(desired_width*winheight > desired_height*winwidth)
 330 |                   desired_height=winheight*desired_width/winwidth;
 331 |                else
 332 |                   desired_width=winwidth*desired_height/winheight;
 333 |                left+=0.5*(x+clickx)*height/winheight-0.5*desired_width;
 334 |                bottom+=(winheight-0.5*(y+clicky))*height/winheight-0.5*desired_height;
 335 |                height=desired_height;
 336 |             }else{
 337 |                // zoom in by some constant factor on the mouse click
 338 |                float factor=0.70710678118654752440084;
 339 |                left+=(1-factor)*height*(x/(float)winheight);
 340 |                bottom+=(1-factor)*height*(1-y/(float)winheight);
 341 |                height*=factor;
 342 |             }
 343 |             glutPostRedisplay();
 344 |             break;
 345 |          case ZOOM_OUT:
 346 |             // zoom out by some constant factor
 347 |             {
 348 |                float factor=1.41421356237309504880168;
 349 |                left-=0.5*(factor-1)*winwidth*height/winheight;
 350 |                bottom-=0.5*(factor-1)*height;
 351 |                height*=factor;
 352 |             }
 353 |             glutPostRedisplay();
 354 |             break;
 355 |          default:
 356 |             ;// nothing to do
 357 |       }
 358 |       action_mode=INACTIVE;
 359 | 
 360 |    }else if(button==GLUT_LEFT_BUTTON)
 361 |       action_mode=PAN;
 362 |    else if(button==GLUT_MIDDLE_BUTTON){
 363 |       clickx=x;
 364 |       clicky=y;
 365 |       action_mode=ZOOM_IN;
 366 |    }else if(button==GLUT_RIGHT_BUTTON)
 367 |       action_mode=ZOOM_OUT;
 368 |    moved_since_mouse_down=false;
 369 |    oldmousex=x;
 370 |    oldmousey=y;
 371 | }
 372 | 
 373 | void PanZoom2D::
 374 | drag(int x, int y)
 375 | {
 376 |    if(x!=oldmousex || y!=oldmousey){
 377 |       moved_since_mouse_down=true;
 378 |       if(action_mode==PAN){
 379 |          float r=height/winheight;
 380 |          left-=r*(x-oldmousex);
 381 |          bottom+=r*(y-oldmousey);
 382 |          glutPostRedisplay();
 383 |       }else if(action_mode==ZOOM_IN)
 384 |          glutPostRedisplay();
 385 |       oldmousex=x;
 386 |       oldmousey=y;
 387 |    }
 388 | }
 389 | 
 390 | void PanZoom2D::
 391 | return_to_default(void)
 392 | {
 393 |    bottom=default_bottom;
 394 |    left=default_left;
 395 |    height=default_height;
 396 | }
 397 | 
 398 | void PanZoom2D::
 399 | transform_mouse(int x, int y, float coords[2])
 400 | {
 401 |    float r=height/winheight;
 402 |    coords[0]=x*r+left;
 403 |    coords[1]=(winheight-y)*r+bottom;
 404 | }
 405 | 
 406 | void PanZoom2D::
 407 | gl_transform(void)
 408 | {
 409 |    glViewport(0, 0, (GLsizei)winwidth, (GLsizei)winheight);
 410 |    glMatrixMode(GL_PROJECTION);
 411 |    glLoadIdentity();
 412 |    glOrtho(left, left+(height*winwidth)/winheight, bottom, bottom+height, 0, 1);
 413 |    glMatrixMode(GL_MODELVIEW);
 414 |    glLoadIdentity();
 415 | }
 416 | 
 417 | void PanZoom2D::
 418 | export_rib(ostream &output)
 419 | {
 420 |    // no projection matrix
 421 |    output<<"Clipping 1 2000"<<endl; // somewhat arbitrary - hopefully this is plenty of space
 422 |    output<<"ReverseOrientation"<<endl;  // RenderMan has a different handedness from OpenGL's default
 423 |    output<<"Scale 1 1 -1"<<endl;        // so we need to correct for that here
 424 |    // scale so that smaller dimension gets scaled to size 2
 425 |    float scalefactor;
 426 |    if(winwidth>winheight) scalefactor=2.0/height;
 427 |    else scalefactor=2.0/(winwidth*height/winheight);
 428 |    output<<"Scale "<<scalefactor<<" "<<scalefactor<<" 1"<<endl;
 429 |    // translate so centre of view gets mapped to (0,0,1000)
 430 |    output<<"Translate "<<-(left+0.5*winwidth*height/winheight)<<" "<<-(bottom+0.5*height)<< " 1000"<<endl;
 431 | }
 432 | 
 433 | void PanZoom2D::
 434 | display_screen(void)
 435 | {
 436 |    if(action_mode==ZOOM_IN && moved_since_mouse_down){
 437 |       glColor3f(1,1,1);
 438 |       glBegin(GL_LINE_STRIP);
 439 |       glVertex2i(clickx, winheight-clicky);
 440 |       glVertex2i(oldmousex, winheight-clicky);
 441 |       glVertex2i(oldmousex, winheight-oldmousey);
 442 |       glVertex2i(clickx, winheight-oldmousey);
 443 |       glVertex2i(clickx, winheight-clicky);
 444 |       glEnd();
 445 |    }
 446 | }
 447 | 
 448 | //=================================================================================
 449 | 
 450 | StaticText::
 451 | StaticText(const char *text_)
 452 |    : text(text_)
 453 | {}
 454 | 
 455 | void StaticText::
 456 | display(int x, int y)
 457 | {
 458 |    dispx=x;
 459 |    dispy=y;
 460 |    width=glutBitmapLength(GLUT_BITMAP_HELVETICA_12, (const unsigned char*)text)+1;
 461 |    height=15;
 462 |    glColor3f(0.3, 0.3, 0.3);
 463 |    glRasterPos2i(x, y-height+2);
 464 |    for(int i=0; text[i]!=0; ++i)
 465 |       glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, text[i]);
 466 |    glColor3f(1, 1, 1);
 467 |    glRasterPos2i(x+1, y-height+3);
 468 |    for(int i=0; text[i]!=0; ++i)
 469 |       glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, text[i]);
 470 | }
 471 | 
 472 | //=================================================================================
 473 | 
 474 | Button::
 475 | Button(const char *text_, int minwidth_)
 476 |    : status(UNINVOLVED), text(text_), minwidth(minwidth_)
 477 | {}
 478 | 
 479 | void Button::
 480 | display(int x, int y)
 481 | {
 482 |    dispx=x;
 483 |    dispy=y;
 484 |    int textwidth=glutBitmapLength(GLUT_BITMAP_HELVETICA_12, (const unsigned char*)text);
 485 |    if(textwidth<minwidth) width=minwidth+24;
 486 |    else width=textwidth+24;
 487 |    height=17;
 488 |    if(status==UNINVOLVED){
 489 |       glColor3f(0.7, 0.7, 0.7);
 490 |       glBegin(GL_QUADS);
 491 |       glVertex2i(x+1, y-1);
 492 |       glVertex2i(x+width, y-1);
 493 |       glVertex2i(x+width, y-height+1);
 494 |       glVertex2i(x+1, y-height+1);
 495 |       glEnd();
 496 |       glColor3f(0.3, 0.3, 0.3);
 497 |       glLineWidth(1);
 498 |       glBegin(GL_LINE_STRIP);
 499 |       glVertex2i(x, y-2);
 500 |       glVertex2i(x, y-height);
 501 |       glVertex2i(x+width-1, y-height);
 502 |       glEnd();
 503 |       glColor3f(0.3, 0.3, 0.3);
 504 |    }else{
 505 |       if(status==SELECTED) glColor3f(0.8, 0.8, 0.8);
 506 |       else                 glColor3f(1, 1, 1);
 507 |       glBegin(GL_QUADS);
 508 |       glVertex2i(x, y-1);
 509 |       glVertex2i(x+width, y-1);
 510 |       glVertex2i(x+width, y-height);
 511 |       glVertex2i(x, y-height);
 512 |       glEnd();
 513 |       glColor3f(0, 0, 0);
 514 |    }
 515 |    glRasterPos2i(x+(width-textwidth)/2, y-height+5);
 516 |    for(int i=0; text[i]!=0; ++i)
 517 |       glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, text[i]);
 518 | }
 519 | 
 520 | bool Button::
 521 | click(int state, int x, int y)
 522 | {
 523 |    if(state==GLUT_DOWN && x>dispx && x<=dispx+width && y<dispy-2 && y>=dispy-height){
 524 |       status=HIGHLIGHTED;
 525 |       glutPostRedisplay();
 526 |       return true;
 527 |    }else if(state==GLUT_UP && status!=UNINVOLVED){
 528 |       status=UNINVOLVED;
 529 |       glutPostRedisplay();
 530 |       if(x>=dispx && x<dispx+width && y<dispy-2 && y>=dispy-height)
 531 |          action();
 532 |       return true;
 533 |    }else
 534 |       return false;
 535 | }
 536 | 
 537 | void Button::
 538 | drag(int x, int y)
 539 | {
 540 |    // needs to control highlighting (SELECTED vs. HIGHLIGHTED)
 541 |    if(status==SELECTED && x>=dispx && x<dispx+width && y<dispy-2 && y>=dispy-height){
 542 |       status=HIGHLIGHTED;
 543 |       glutPostRedisplay();
 544 |    }else if(status==HIGHLIGHTED && !(x>=dispx && x<dispx+width && y<dispy-2 && y>=dispy-height)){
 545 |       status=SELECTED;
 546 |       glutPostRedisplay();
 547 |    }
 548 | }
 549 | 
 550 | //=================================================================================
 551 | 
 552 | Slider::
 553 | Slider(const char *text_, int length_, int position_, int justify_)
 554 |    : status(UNINVOLVED), text(text_), length(length_), justify(justify_), position(position_)
 555 | {}
 556 | 
 557 | void Slider::
 558 | display(int x, int y)
 559 | {
 560 |    dispx=x;
 561 |    dispy=y;
 562 |    width=glutBitmapLength(GLUT_BITMAP_HELVETICA_12, (const unsigned char*)text);
 563 |    if(width<justify) width=justify;
 564 |    width+=11+6+length+1;
 565 |    height=15;
 566 |    glColor3f(0.3, 0.3, 0.3);
 567 |    glRasterPos2i(x, y-height+2);
 568 |    for(int i=0; text[i]!=0; ++i)
 569 |       glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, text[i]);
 570 |    glColor3f(1, 1, 1);
 571 |    glRasterPos2i(x+1, y-height+3);
 572 |    for(int i=0; text[i]!=0; ++i)
 573 |       glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, text[i]);
 574 |    scrollxmin=x+width-length-12;
 575 |    scrollxmax=x+width;
 576 |    scrollymin=y-height+1;
 577 |    scrollymax=y-2;
 578 |    glColor3f(0.3, 0.3, 0.3);
 579 |    glLineWidth(1);
 580 |    glBegin(GL_LINE_STRIP);
 581 |    glVertex2i(scrollxmin, scrollymax-1);
 582 |    glVertex2i(scrollxmin, scrollymin);
 583 |    glVertex2i(scrollxmax-1, scrollymin);
 584 |    glVertex2i(scrollxmax-1, scrollymax-1);
 585 |    glEnd();
 586 |    glColor3f(0.7, 0.7, 0.7);
 587 |    glBegin(GL_LINE_STRIP);
 588 |    glVertex2i(scrollxmin+1, scrollymax);
 589 |    glVertex2i(scrollxmin+1, scrollymin+1);
 590 |    glVertex2i(scrollxmax, scrollymin+1);
 591 |    glVertex2i(scrollxmax, scrollymax);
 592 |    glEnd();
 593 |    if(status==UNINVOLVED){
 594 |       glColor3f(0.3, 0.3, 0.3);
 595 |       glBegin(GL_LINE_STRIP);
 596 |       glVertex2i(scrollxmin+position+2, scrollymax-2);
 597 |       glVertex2i(scrollxmin+position+2, scrollymin+2);
 598 |       glVertex2i(scrollxmin+position+10, scrollymin+2);
 599 |       glEnd();
 600 |       glColor3f(0.7, 0.7, 0.7);
 601 |       glBegin(GL_QUADS);
 602 |       glVertex2i(scrollxmin+position+3, scrollymin+3);
 603 |       glVertex2i(scrollxmin+position+11, scrollymin+3);
 604 |       glVertex2i(scrollxmin+position+11, scrollymax);
 605 |       glVertex2i(scrollxmin+position+3, scrollymax);
 606 |       glEnd();
 607 |    }else{ // SELECTED
 608 |       glColor3f(1, 1, 1);
 609 |       glBegin(GL_QUADS);
 610 |       glVertex2i(scrollxmin+position+2, scrollymin+2);
 611 |       glVertex2i(scrollxmin+position+11, scrollymin+2);
 612 |       glVertex2i(scrollxmin+position+11, scrollymax);
 613 |       glVertex2i(scrollxmin+position+2, scrollymax);
 614 |       glEnd();
 615 |    }
 616 | }
 617 | 
 618 | bool Slider::
 619 | click(int state, int x, int y)
 620 | {
 621 |    if(state==GLUT_DOWN && x>scrollxmin+position+2 && x<=scrollxmin+position+11 && y<scrollymax-1 && y>=scrollymin+2){
 622 |       status=SELECTED;
 623 |       clickx=x;
 624 |       glutPostRedisplay();
 625 |       return true;
 626 |    }else if(status!=UNINVOLVED && state==GLUT_UP){
 627 |       status=UNINVOLVED;
 628 |       glutPostRedisplay();
 629 |       return true;
 630 |    }else
 631 |       return false;
 632 | }
 633 | 
 634 | void Slider::
 635 | drag(int x, int y)
 636 | {
 637 |    if(status==SELECTED){
 638 |       glutPostRedisplay();
 639 |       int newposition=position+(x-clickx);
 640 |       clickx=x;
 641 |       if(newposition<0){
 642 |          clickx+=(0-newposition);
 643 |          newposition=0;
 644 |       }else if(newposition>length){
 645 |          clickx+=(length-newposition);
 646 |          newposition=length;
 647 |       }
 648 |       if(newposition!=position){
 649 |          position=newposition;
 650 |          action();
 651 |          glutPostRedisplay();
 652 |       }
 653 |    }
 654 | }
 655 | 
 656 | //=================================================================================
 657 | 
 658 | WidgetList::
 659 | WidgetList(int indent_, bool hidden_)
 660 |    : indent(indent_), hidden(hidden_), downclicked_member(-1)
 661 | {
 662 | }
 663 | 
 664 | void WidgetList::
 665 | display(int x, int y)
 666 | {
 667 |    dispx=x;
 668 |    dispy=y;
 669 |    if(hidden){
 670 |       width=height=0;
 671 |    }else{
 672 |       height=0;
 673 |       for(unsigned int i=0; i<list.size(); ++i){
 674 |          list[i]->display(x+indent, y-height);
 675 |          height+=list[i]->height;
 676 |          width=(width<indent+list[i]->width) ? indent+list[i]->width : width;
 677 |       }
 678 |    }
 679 | }
 680 | 
 681 | bool WidgetList::
 682 | click(int state, int x, int y)
 683 | {
 684 |    //if(hidden || x<dispx || x>=dispx+width || y>=dispy || y<dispy-height) return false; // early exit
 685 |    if(state==GLUT_DOWN){ // search for correct widget
 686 |       for(unsigned int i=0; i<list.size(); ++i){
 687 |          if(list[i]->click(state, x, y)){
 688 |             downclicked_member=i;
 689 |             return true;
 690 |          }
 691 |       }
 692 |    }else if(state==GLUT_UP && downclicked_member>=0){
 693 |       list[downclicked_member]->click(state, x, y);
 694 |       downclicked_member=-1;
 695 |    }
 696 |    return false;
 697 | }
 698 | 
 699 | void WidgetList::
 700 | drag(int x, int y)
 701 | {
 702 |    if(downclicked_member>=0)
 703 |       list[downclicked_member]->drag(x, y);
 704 | }
 705 | 
 706 | //=================================================================================
 707 | 
 708 | static void gluviReshape(int w, int h)
 709 | {
 710 |    winwidth=w;
 711 |    winheight=h;
 712 |    glutPostRedisplay(); // triggers the camera to adjust itself to the new dimensions
 713 | }
 714 | 
 715 | //=================================================================================
 716 | 
 717 | static void gluviDisplay()
 718 | {
 719 |    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
 720 | 
 721 |    // draw the scene
 722 |    if(camera) camera->gl_transform();
 723 |    if(userDisplayFunc) userDisplayFunc();
 724 | 
 725 |    // now draw widgets on top
 726 |    glPushAttrib(GL_CURRENT_BIT|GL_ENABLE_BIT|GL_LINE_BIT);
 727 |    glDisable(GL_DEPTH_TEST);
 728 |    glDisable(GL_LIGHTING);
 729 |    glLineWidth(1);
 730 |    // and probably more needs setting before widgets
 731 | 
 732 |    glMatrixMode(GL_MODELVIEW);
 733 |    glLoadIdentity();
 734 |    glMatrixMode(GL_PROJECTION);
 735 |    glLoadIdentity();
 736 |    gluOrtho2D(0, winwidth, 0, winheight);
 737 | 
 738 |    root.display(0, winheight);
 739 | 
 740 |    // and allow the camera to draw something on screen (e.g. for zooming extent)
 741 |    if(camera) camera->display_screen();
 742 | 
 743 |    glPopAttrib();
 744 | 
 745 |    glutSwapBuffers();
 746 | }
 747 | 
 748 | //=================================================================================
 749 | 
 750 | static enum {NOBODY, CAMERA, WIDGETS, USER} mouse_owner=NOBODY;
 751 | 
 752 | static void gluviMouse(int button, int state, int x, int y)
 753 | {
 754 |    if(state==GLUT_DOWN){
 755 |       int mods=glutGetModifiers();
 756 |       if(camera && mods==GLUT_ACTIVE_SHIFT){
 757 |          camera->click(button, state, x, y);
 758 |          mouse_owner=CAMERA;
 759 |       }else if(button==GLUT_LEFT_BUTTON && root.click(state, x, winheight-y)){
 760 |          mouse_owner=WIDGETS;
 761 |       }else if(userMouseFunc){
 762 |          userMouseFunc(button, state, x, y);
 763 |          mouse_owner=USER;
 764 |       }
 765 |    }else{ // mouse up - send event to whoever got the mouse down
 766 |       switch(mouse_owner){
 767 |          case CAMERA:
 768 |             camera->click(button, state, x, y);
 769 |             break;
 770 |          case WIDGETS:
 771 |             root.click(state, x, winheight-y);
 772 |             break;
 773 |          case USER:
 774 |             if(userMouseFunc) userMouseFunc(button, state, x, y);
 775 |             break;
 776 |          default:
 777 |            ;// nothing to do
 778 |       }
 779 |       mouse_owner=NOBODY;
 780 |    }
 781 | }
 782 | 
 783 | //=================================================================================
 784 | 
 785 | static void gluviDrag(int x, int y)
 786 | {
 787 |    switch(mouse_owner){
 788 |       case CAMERA:
 789 |          camera->drag(x, y);
 790 |          break;
 791 |       case WIDGETS:
 792 |          root.drag(x, winheight-y);
 793 |          break;
 794 |       case USER:
 795 |          if(userDragFunc) userDragFunc(x, y);
 796 |          break;
 797 |       default:
 798 |          ;// nothing to do
 799 |    }
 800 | }
 801 | 
 802 | //=================================================================================
 803 | 
 804 | void ppm_screenshot(const char *filename_format, ...)
 805 | {
 806 |    va_list ap;
 807 |    va_start(ap, filename_format);
 808 | #ifdef _MSC_VER
 809 | #define FILENAMELENGTH 256
 810 |    char filename[FILENAMELENGTH];
 811 |    _vsnprintf(filename, FILENAMELENGTH, filename_format, ap);
 812 |    ofstream out(filename, ofstream::binary);
 813 | #else
 814 |    char *filename;
 815 |    vasprintf(&filename, filename_format, ap);
 816 |    ofstream out(filename, ofstream::binary);
 817 |    free(filename);
 818 | #endif
 819 |    if(!out) return;
 820 |    GLubyte *image_buffer=new GLubyte[3*winwidth*winheight];
 821 |    glReadBuffer(GL_FRONT);
 822 |    glReadPixels(0, 0, winwidth, winheight, GL_RGB, GL_UNSIGNED_BYTE, image_buffer);
 823 |    out<<"P6\n"<<winwidth<<' '<<winheight<<" 255\n";
 824 |    for(int i=1; i<=winheight; ++i)
 825 |       out.write((const char*)image_buffer+3*winwidth*(winheight-i), 3*winwidth);
 826 |    delete[] image_buffer;
 827 | }
 828 | 
 829 | static void write_big_endian_ushort(std::ostream &output, unsigned short v)
 830 | {
 831 |    output.put((v>>8)%256);
 832 |    output.put(v%256);
 833 | }
 834 | 
 835 | static void write_big_endian_uint(std::ostream &output, unsigned int v)
 836 | {
 837 |    output.put((v>>24)%256);
 838 |    output.put((v>>16)%256);
 839 |    output.put((v>>8)%256);
 840 |    output.put(v%256);
 841 | }
 842 | 
 843 | void sgi_screenshot(const char *filename_format, ...)
 844 | {
 845 |    va_list ap;
 846 |    va_start(ap, filename_format);
 847 | #ifdef _MSC_VER
 848 | #define FILENAMELENGTH 256
 849 |    char filename[FILENAMELENGTH];
 850 |    _vsnprintf(filename, FILENAMELENGTH, filename_format, ap);
 851 |    ofstream output(filename, ofstream::binary);
 852 | #else
 853 |    char *filename;
 854 |    vasprintf(&filename, filename_format, ap);
 855 |    ofstream output(filename, ofstream::binary);
 856 | #endif
 857 |    if(!output) return;
 858 |    // first write the SGI header
 859 |    write_big_endian_ushort(output, 474); // magic number to identify this as an SGI image file
 860 |    output.put(0); // uncompressed
 861 |    output.put(1); // use 8-bit colour depth
 862 |    write_big_endian_ushort(output, 3); // number of dimensions
 863 |    write_big_endian_ushort(output, winwidth); // x size
 864 |    write_big_endian_ushort(output, winheight); // y size
 865 |    write_big_endian_ushort(output, 3); // three colour channels (z size)
 866 |    write_big_endian_uint(output, 0); // minimum pixel value
 867 |    write_big_endian_uint(output, 255); // maximum pixel value
 868 |    write_big_endian_uint(output, 0); // dummy spacing
 869 |    // image name
 870 |    int i;
 871 |    for(i=0; i<80 && filename[i]; ++i)
 872 |       output.put(filename[i]);
 873 |    for(; i<80; ++i)
 874 |       output.put(0);
 875 |    write_big_endian_uint(output, 0); // colormap is normal
 876 |    for(i=0; i<404; ++i) output.put(0); // filler to complete header
 877 |    // now write the SGI image data
 878 |    GLubyte *image_buffer=new GLubyte[winwidth*winheight];
 879 |    glReadBuffer(GL_FRONT);
 880 |    glReadPixels(0, 0, winwidth, winheight, GL_RED, GL_UNSIGNED_BYTE, image_buffer);
 881 |    output.write((const char*)image_buffer, winwidth*winheight);
 882 |    glReadPixels(0, 0, winwidth, winheight, GL_GREEN, GL_UNSIGNED_BYTE, image_buffer);
 883 |    output.write((const char*)image_buffer, winwidth*winheight);
 884 |    glReadPixels(0, 0, winwidth, winheight, GL_BLUE, GL_UNSIGNED_BYTE, image_buffer);
 885 |    output.write((const char*)image_buffer, winwidth*winheight);
 886 |    delete[] image_buffer;
 887 | #ifndef _MSC_VER
 888 |    free(filename);
 889 | #endif
 890 | }
 891 | 
 892 | void set_generic_lights(void)
 893 | {
 894 |    glEnable(GL_LIGHTING);
 895 |    {
 896 |       GLfloat ambient[4] = {.3, .3, .3, 1};
 897 |       glLightModelfv (GL_LIGHT_MODEL_AMBIENT,ambient);
 898 |    }
 899 |    {
 900 |       GLfloat color[4] = {.8, .8, .8, 1};
 901 |       glLightfv (GL_LIGHT0, GL_DIFFUSE, color);
 902 |       glLightfv (GL_LIGHT0, GL_SPECULAR, color);
 903 |       glEnable (GL_LIGHT0);
 904 |    }
 905 |    {
 906 |       GLfloat color[4] = {.4, .4, .4, 1};
 907 |       glLightfv (GL_LIGHT1, GL_DIFFUSE, color);
 908 |       glLightfv (GL_LIGHT1, GL_SPECULAR, color);
 909 |       glEnable (GL_LIGHT1);
 910 |    }
 911 |    {
 912 |       GLfloat color[4] = {.2, .2, .2, 1};
 913 |       glLightfv (GL_LIGHT2, GL_DIFFUSE, color);
 914 |       glLightfv (GL_LIGHT2, GL_SPECULAR, color);
 915 |       glEnable (GL_LIGHT2);
 916 |    }
 917 | }
 918 | 
 919 | void set_generic_material(float r, float g, float b, GLenum face)
 920 | {
 921 |    GLfloat ambient[4], diffuse[4], specular[4];
 922 |    ambient[0]=0.1*r+0.03; ambient[1]=0.1*g+0.03; ambient[2]=0.1*b+0.03; ambient[3]=1;
 923 |    diffuse[0]=0.7*r;      diffuse[1]=0.7*g;      diffuse[2]=0.7*b;      diffuse[3]=1;
 924 |    specular[0]=0.1*r+0.1; specular[1]=0.1*g+0.1; specular[2]=0.1*b+0.1; specular[3]=1;
 925 |    glMaterialfv(face, GL_AMBIENT, ambient);
 926 |    glMaterialfv(face, GL_DIFFUSE, diffuse);
 927 |    glMaterialfv(face, GL_SPECULAR, specular);
 928 |    glMaterialf(face, GL_SHININESS, 32);
 929 | }
 930 | 
 931 | void set_matte_material(float r, float g, float b, GLenum face)
 932 | {
 933 |    GLfloat ambient[4], diffuse[4], specular[4];
 934 |    ambient[0]=0.1*r+0.03; ambient[1]=0.1*g+0.03; ambient[2]=0.1*b+0.03; ambient[3]=1;
 935 |    diffuse[0]=0.9*r;      diffuse[1]=0.9*g;      diffuse[2]=0.9*b;      diffuse[3]=1;
 936 |    specular[0]=0;         specular[1]=0;         specular[2]=0;         specular[3]=1;
 937 |    glMaterialfv(face, GL_AMBIENT, ambient);
 938 |    glMaterialfv(face, GL_DIFFUSE, diffuse);
 939 |    glMaterialfv(face, GL_SPECULAR, specular);
 940 | }
 941 | 
 942 | /** 
 943 |  * Draw a vector in 3D.  If arrow_head_length not specified, set it to 10% of vector length.
 944 |  * Mar 29, 2006
 945 |  */
 946 | void draw_3d_arrow(const float base[3], const float point[3], float arrow_head_length)
 947 | {
 948 |     //glPushAttrib(GL_CURRENT_BIT|GL_ENABLE_BIT|GL_LINE_BIT);
 949 |     //glDisable(GL_LIGHTING);
 950 |     glLineWidth(1);
 951 |     glColor3f(0.5,0.5,0.5);
 952 | 
 953 |     Vec3f w(point[0]-base[0], point[1]-base[1], point[2]-base[2]);
 954 |     double len = mag(w);
 955 |     w = w / (float)len;    // normalize to build coordinate system
 956 |     
 957 | 	// create a coordinate system from the vector:
 958 |     // get a vector perp to w and y-axis
 959 |     Vec3f u = cross(w, Vec3f(0,1,0));
 960 |     
 961 |     // If vector is parallel to the y-axis, use the x-axis
 962 |     if (mag(u) == 0)
 963 |         u = cross(w, Vec3f(1,0,0));
 964 | 
 965 |     // get a vector perp to w and u
 966 |     Vec3f v = cross(w, u/mag(u));
 967 |     v = v/mag(v);
 968 | 
 969 |     if (!arrow_head_length)
 970 |         arrow_head_length = 0.1 * len;
 971 |     
 972 |     // arrow head points
 973 |     //@@@@@@@ POSSIBILITY: CREATE FOUR ARROW HEAD SEGMENTS INSTEAD OF TWO
 974 |     Vec3f arrow1, arrow2;
 975 |     arrow1[0] = point[0] + arrow_head_length * (v[0] - w[0]);
 976 |     arrow1[1] = point[1] + arrow_head_length * (v[1] - w[1]);
 977 |     arrow1[2] = point[2] + arrow_head_length * (v[2] - w[2]);
 978 |     arrow2[0] = point[0] + arrow_head_length * (-v[0] - w[0]);
 979 |     arrow2[1] = point[1] + arrow_head_length * (-v[1] - w[1]);
 980 |     arrow2[2] = point[2] + arrow_head_length * (-v[2] - w[2]);
 981 | 
 982 |     glBegin(GL_LINES);
 983 |         glVertex3f(base[0],   base[1],   base[2]);
 984 |         glVertex3f(point[0],  point[1],  point[2]);
 985 |         glVertex3f(point[0],  point[1],  point[2]);
 986 |         glVertex3f(arrow1[0], arrow1[1], arrow1[2]);
 987 |         glVertex3f(point[0],  point[1],  point[2]);
 988 |         glVertex3f(arrow2[0], arrow2[1], arrow2[2]);
 989 |     glEnd();
 990 |     
 991 |     //glPopAttrib();
 992 | }
 993 | 
 994 | // draw_2d_arrow assumptions: 
 995 | // line width, point size, and color are set by the user prior to calling the routine
 996 | void draw_2d_arrow(const Vec2f base, const Vec2f point, float arrow_head_length)
 997 | {
 998 |     //glPushAttrib(GL_CURRENT_BIT|GL_ENABLE_BIT|GL_LINE_BIT);
 999 |     //glDisable(GL_LIGHTING);
1000 | 
1001 |     Vec2f w = point-base;
1002 | 	double len = mag(w);
1003 | 	
1004 | 	if (len==0) {
1005 | 		glBegin(GL_POINTS);
1006 |         glVertex2f(base[0], base[1]);
1007 | 		glEnd();
1008 | 		return;
1009 | 	}
1010 | 
1011 |     w = w / (float)len;    // normalize to build coordinate system
1012 |     
1013 | 	// u = w + 90 
1014 | 	// using rotation matrix  0  1
1015 | 	//	                     -1  0
1016 |     Vec2f u = Vec2f(1*w[1], -1*w[0]);
1017 |     u = u/mag(u);
1018 | 	
1019 | 	// v = w - 90 (in fact v=-u)
1020 |     Vec2f v = Vec2f(-1*w[1], 1*w[0]);
1021 |     v = v/mag(v);
1022 | 
1023 | 	if (!arrow_head_length) {
1024 | 		arrow_head_length = 0.1 * len;
1025 | 	}
1026 |     
1027 | 	// arrow head points
1028 |     Vec2f arrow1, arrow2;
1029 | 	arrow1 = point + arrow_head_length * (v-w);
1030 | 	arrow2 = point + arrow_head_length * (u-w);
1031 | 
1032 |     glBegin(GL_LINES);
1033 |         glVertex2f(base[0], base[1]);
1034 |         glVertex2f(point[0], point[1]);
1035 |         glVertex2f(point[0], point[1]);
1036 |         glVertex2f(arrow1[0], arrow1[1]);
1037 |         glVertex2f(point[0], point[1]);
1038 |         glVertex2f(arrow2[0], arrow2[1]);
1039 |     glEnd();
1040 |     
1041 |     //glPopAttrib();
1042 | }
1043 | 
1044 | 
1045 | void draw_coordinate_grid(float size, int spacing)
1046 | {
1047 |    glPushAttrib(GL_CURRENT_BIT|GL_ENABLE_BIT|GL_LINE_BIT);
1048 |    glDisable(GL_LIGHTING);
1049 |    glLineWidth(1);
1050 | 
1051 |    glBegin(GL_LINES);
1052 |    glColor3f(0.5,0.5,0.5);
1053 |    for(int i=-spacing; i<=spacing; ++i){
1054 |       glVertex3f(-size,0,i*size/spacing);
1055 |       glVertex3f((i!=0)*size,0,i*size/spacing);
1056 |       glVertex3f(i*size/spacing,0,-size);
1057 |       glVertex3f(i*size/spacing,0,(i!=0)*size);
1058 |    }
1059 |    glColor3f(1,0,0);
1060 |    glVertex3f(0,0,0);
1061 |    glVertex3f(size,0,0);
1062 |    glColor3f(0,1,0);
1063 |    glVertex3f(0,0,0);
1064 |    glVertex3f(0,size,0);
1065 |    glColor3f(0,0,1);
1066 |    glVertex3f(0,0,0);
1067 |    glVertex3f(0,0,size);
1068 |    glEnd();
1069 | 
1070 |    glPopAttrib();
1071 | }
1072 | 
1073 | void draw_text(const float point[3], const char *text, int fontsize)
1074 | {
1075 |    // please implement me!
1076 | }
1077 | 
1078 | //=================================================================================
1079 | 
1080 | void init(const char *windowtitle, int *argc, char **argv)
1081 | {
1082 |    glutInit(argc, argv);
1083 |    glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA|GLUT_DEPTH);
1084 |    glutInitWindowSize(winwidth, winheight);
1085 |    glutCreateWindow(windowtitle);
1086 |    glutReshapeFunc(gluviReshape);
1087 |    glutDisplayFunc(gluviDisplay);
1088 |    glutMouseFunc(gluviMouse);
1089 |    glutMotionFunc(gluviDrag);
1090 |    glEnable(GL_DEPTH_TEST);
1091 |    glClearColor(0, 0, 0, 0);
1092 |    glClearDepth(1);
1093 |    glPixelStorei(GL_PACK_ALIGNMENT, 1);
1094 |    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
1095 | }
1096 | 
1097 | //=================================================================================
1098 | 
1099 | void (*userDisplayFunc)(void)=0; 
1100 | void (*userMouseFunc)(int button, int state, int x, int y)=0;
1101 | void (*userDragFunc)(int x, int y)=0;
1102 | Camera *camera=0;
1103 | WidgetList root(0);
1104 | int winwidth=720, winheight=480;
1105 | 
1106 | //=================================================================================
1107 | 
1108 | void run(void)
1109 | {
1110 |    glutMainLoop();
1111 | }
1112 | 
1113 | };
1114 | 
1115 | 


--------------------------------------------------------------------------------
/gluvi.h:
--------------------------------------------------------------------------------
  1 | #ifndef GLUVI_H
  2 | #define GLUVI_H
  3 | 
  4 | #include <iostream>
  5 | #include <vector>
  6 | 
  7 | #ifdef __APPLE__
  8 | #include <GLUT/glut.h> // why does Apple have to put glut.h here...
  9 | #else
 10 | #include <GL/glut.h> // ...when everyone else puts it here?
 11 | #endif
 12 | 
 13 | #include "vec.h"
 14 | 
 15 | namespace Gluvi{
 16 | 
 17 | /*
 18 | For portions of the code which export RIB commands (e.g. from cameras):
 19 | 
 20 | The RenderMan (R) Interface Procedures and Protocol are:
 21 | Copyright 1988, 1989, Pixar
 22 | All Rights Reserved
 23 | */
 24 | 
 25 | // the camera capability for Gluvi
 26 | struct Camera
 27 | {
 28 |    virtual ~Camera(void) {}
 29 |    virtual void click(int button, int state, int x, int y) = 0;
 30 |    virtual void drag(int x, int y) = 0;
 31 |    virtual void return_to_default(void) = 0;
 32 |    //@@@ add these to be called by a user glutKeyboardFunc() thing so that
 33 |    //@@@ cameras can easily add keyboard shortcuts for e.g. return_to_default, transformation, etc.
 34 |    //virtual void navigation_keyboard_handler(unsigned char key, int x, int y) = 0;
 35 |    //virtual void navigation_special_key_handler(int key, int x, int y) = 0;
 36 |    virtual void gl_transform(void) = 0;
 37 |    virtual void export_rib(std::ostream &output) = 0;
 38 |    virtual void display_screen(void) {} // in case the camera needs to show anything on screen
 39 | };
 40 | 
 41 | struct Target3D : public Camera
 42 | {
 43 |    float target[3], dist;
 44 |    float heading, pitch;
 45 |    float default_target[3], default_dist;
 46 |    float default_heading, default_pitch;
 47 |    float fovy;
 48 |    float near_clip_factor, far_clip_factor;
 49 |    enum {INACTIVE, ROTATE, TRUCK, DOLLY} action_mode;
 50 |    int oldmousex, oldmousey;
 51 | 
 52 |    Target3D(float target_[3]=0, float dist_=1, float heading_=0, float pitch_=0, float fovy_=45,
 53 |             float near_clip_factor_=0.01, float far_clip_factor_=100);
 54 |    void click(int button, int state, int x, int y);
 55 |    void drag(int x, int y);
 56 |    void return_to_default(void);
 57 |    void transform_mouse(int x, int y, float ray_origin[3], float ray_direction[3]);
 58 |    void get_viewing_direction(float direction[3]);
 59 |    void gl_transform(void);
 60 |    void export_rib(std::ostream &output);
 61 | };
 62 | 
 63 | // same as above, but with orthographic projection
 64 | struct TargetOrtho3D : public Camera
 65 | {
 66 |    float target[3], dist;
 67 |    float heading, pitch;
 68 |    float default_target[3], default_dist;
 69 |    float default_heading, default_pitch;
 70 |    float height_factor;
 71 |    float near_clip_factor, far_clip_factor;
 72 |    enum {INACTIVE, ROTATE, TRUCK, DOLLY} action_mode;   // @@@@ WHAT ABOUT ZOOMING??? IS WIDTH ALWAYS A FUNCTION OF DIST?
 73 |    int oldmousex, oldmousey;
 74 | 
 75 |    TargetOrtho3D(float target_[3]=0, float dist_=1, float heading_=0, float pitch_=0, float height_factor_=1,
 76 |                  float near_clip_factor_=0.01, float far_clip_factor_=100);
 77 |    void click(int button, int state, int x, int y);
 78 |    void drag(int x, int y);
 79 |    void return_to_default(void);
 80 |    void transform_mouse(int x, int y, float ray_origin[3], float ray_direction[3]);
 81 |    void get_viewing_direction(float direction[3]);
 82 |    void gl_transform(void);
 83 |    void export_rib(std::ostream &output);
 84 | };
 85 | 
 86 | struct PanZoom2D : public Camera
 87 | {
 88 |    float bottom, left, height;
 89 |    float default_bottom, default_left, default_height;
 90 |    enum {INACTIVE, PAN, ZOOM_IN, ZOOM_OUT} action_mode;
 91 |    int oldmousex, oldmousey;
 92 |    bool moved_since_mouse_down; // to distinuish simple clicks from drags
 93 |    int clickx, clicky;
 94 | 
 95 |    PanZoom2D(float bottom_=0, float left_=0, float height_=1);
 96 |    void click(int button, int state, int x, int y);
 97 |    void drag(int x, int y);
 98 |    void return_to_default(void);
 99 |    void transform_mouse(int x, int y, float coords[2]);
100 |    void gl_transform(void);
101 |    void export_rib(std::ostream &output);
102 |    void display_screen(void);
103 | };
104 | 
105 | // overlaid user-interface widgets
106 | struct Widget
107 | {
108 |    int dispx, dispy, width, height; // set in display()
109 | 
110 |    virtual ~Widget() {}
111 |    virtual void display(int x, int y) = 0;
112 |    virtual bool click(int state, int x, int y) { return false; } // returns true if click handled by widget
113 |    virtual void drag(int x, int y) {}
114 | };
115 | 
116 | struct StaticText : public Widget
117 | {
118 |    const char *text;
119 | 
120 |    StaticText(const char *text_);
121 |    void display(int x, int y);
122 | };
123 | 
124 | struct Button : public Widget
125 | {
126 |    enum {UNINVOLVED, SELECTED, HIGHLIGHTED} status;
127 |    const char *text;
128 |    int minwidth;
129 | 
130 |    Button(const char *text_, int minwidth_=0);
131 |    void display(int x, int y);
132 |    bool click(int state, int x, int y);
133 |    void drag(int x, int y);
134 |    virtual void action() {}
135 | };
136 | 
137 | struct Slider : public Widget
138 | {
139 |    enum {UNINVOLVED, SELECTED} status;
140 |    const char *text;
141 |    int length, justify;
142 |    int position;
143 |    int scrollxmin, scrollxmax, scrollymin, scrollymax;
144 |    int clickx;
145 | 
146 |    Slider(const char *text_, int length_=100, int position_=0, int justify_=0);
147 |    void display(int x, int y);
148 |    bool click(int state, int x, int y);
149 |    void drag(int x, int y);
150 |    virtual void action() {}
151 | };
152 | 
153 | struct WidgetList : public Widget
154 | {
155 |    int indent;
156 |    bool hidden;
157 |    std::vector<Widget*> list;
158 |    int downclicked_member;
159 | 
160 |    WidgetList(int listindent_=12, bool hidden_=false);
161 |    void display(int x, int y);
162 |    bool click(int state, int x, int y);
163 |    void drag(int x, int y);
164 | };
165 | 
166 | // display callback
167 | extern void (*userDisplayFunc)(void); 
168 | 
169 | // mouse callbacks for events that Gluvi ignores (control not pressed, or mouse not on an active widget)
170 | extern void (*userMouseFunc)(int button, int state, int x, int y);
171 | extern void (*userDragFunc)(int x, int y);
172 | 
173 | // user is free to do their own callbacks for everything else except glutReshape()
174 | 
175 | // additional helpful functions
176 | void ppm_screenshot(const char *filename_format, ...);
177 | void sgi_screenshot(const char *filename_format, ...);
178 | void set_generic_lights(void);
179 | void set_generic_material(float r, float g, float b, GLenum face=GL_FRONT_AND_BACK);
180 | void set_matte_material(float r, float g, float b, GLenum face=GL_FRONT_AND_BACK);
181 | //@@@@@@@ USEFUL FUNCTIONALITY:
182 | void draw_3d_arrow(const float base[3], const float point[3], float arrow_head_length=0);
183 | void draw_2d_arrow(const Vec2f base, const Vec2f point, float arrow_head_length);
184 | void draw_coordinate_grid(float size=1, int spacing=10);
185 | void draw_text(const float point[3], const char *text, int fontsize=12);
186 | 
187 | // call init first thing
188 | void init(const char *windowtitle, int *argc, char **argv);
189 | 
190 | // the Gluvi state
191 | extern Camera *camera;
192 | extern WidgetList root;
193 | extern int winwidth, winheight;
194 | 
195 | // then after setting the Gluvi state and doing any of your own set-up, call run()
196 | void run(void);
197 | 
198 | }; // end of namespace
199 | 
200 | #endif
201 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <cstdio>
  2 | #include <iostream>
  3 | #include <sstream>
  4 | #include <fstream>
  5 | #include <string>
  6 | #include <cfloat>
  7 | 
  8 | #include "gluvi.h"
  9 | #include "fluidsim.h"
 10 | #include "openglutils.h"
 11 | #include "array2_utils.h"
 12 | 
 13 | using namespace std;
 14 | 
 15 | //Try changing the grid resolution
 16 | int grid_resolution = 100;
 17 | float timestep = 0.002f;
 18 | 
 19 | //Display properties
 20 | bool draw_grid = false;
 21 | bool draw_particles = true;
 22 | bool draw_velocities = false;
 23 | bool draw_boundaries = true;
 24 | 
 25 | float grid_width = 1;
 26 | 
 27 | FluidSim sim;
 28 | 
 29 | //Gluvi stuff
 30 | //-------------
 31 | Gluvi::PanZoom2D cam(-0.1f, -0.35f, 1.2f);
 32 | double oldmousetime;
 33 | Vec2f oldmouse;
 34 | void display();
 35 | void mouse(int button, int state, int x, int y);
 36 | void drag(int x, int y);
 37 | void timer(int junk);
 38 | 
 39 | //Boundary definition - several circles in a circular domain.
 40 | 
 41 | Vec2f c0(0.5f,0.5f), c1(0.7f,0.5f), c2(0.3f,0.35f), c3(0.5f,0.7f);
 42 | float rad0 = 0.4f,  rad1 = 0.1f,  rad2 = 0.1f,   rad3 = 0.1f;
 43 | 
 44 | float circle_phi(const Vec2f& position, const Vec2f& centre, float radius) {
 45 |    return (dist(position,centre) - radius);
 46 | }
 47 | 
 48 | float boundary_phi(const Vec2f& position) {
 49 |    float phi0 = -circle_phi(position, c0, rad0);
 50 |    float phi1 = circle_phi(position, c1, rad1);
 51 |    float phi2 = circle_phi(position, c2, rad2);
 52 |    float phi3 = circle_phi(position, c3, rad3);
 53 |    
 54 |    return phi0;//min(min(phi0,phi1),min(phi2,phi3));
 55 | }
 56 | 
 57 | 
 58 | //Main testing code
 59 | //-------------
 60 | int main(int argc, char **argv)
 61 | {
 62 |    
 63 |    //Setup viewer stuff
 64 |    Gluvi::init("GFM Free Surface Liquid Solver with Static Variational Boundaries", &argc, argv);
 65 |    Gluvi::camera=&cam;
 66 |    Gluvi::userDisplayFunc=display;
 67 |    Gluvi::userMouseFunc=mouse;
 68 |    Gluvi::userDragFunc=drag;
 69 |    glClearColor(1,1,1,1);
 70 |    
 71 |    glutTimerFunc(1000, timer, 0);
 72 |    
 73 |    //Set up the simulation
 74 |    sim.initialize(grid_width, grid_resolution, grid_resolution);
 75 |    
 76 |    //set up a circle boundary
 77 |    sim.set_boundary(boundary_phi);
 78 |    
 79 |    //Stick some liquid particles in the domain
 80 |    int offset = 0;
 81 |    for(int i = 0; i < sqr(grid_resolution); ++i) {
 82 |       for(int parts = 0; parts < 3; ++parts) {
 83 |          float x = randhashf(++offset, 0,1);
 84 |          float y = randhashf(++offset, 0,1);
 85 |          Vec2f pt(x,y);
 86 |          
 87 |          //add a column (for buckling) and a beam (for bending) and a disk (for rolling and flowing)
 88 |          if(boundary_phi(pt) > 0 && (pt[0] > 0.42f && pt[0] < 0.46f || pt[0] < 0.36 && pt[1] > 0.45f && pt[1] < 0.5f || circle_phi(pt, Vec2f(0.7f, 0.65f), 0.15f) < 0))
 89 |             sim.add_particle(pt);
 90 |          
 91 | 
 92 |       }
 93 |    }
 94 | 
 95 | 
 96 | 
 97 |    Gluvi::run();
 98 | 
 99 |    return 0;
100 | }
101 | 
102 | 
103 | void display(void)
104 | {
105 |   
106 |    if(draw_grid) {
107 |       glColor3f(0,0,0);
108 |       glLineWidth(1);
109 |       draw_grid2d(Vec2f(0,0), sim.dx, sim.ni, sim.nj);  
110 |    }
111 | 
112 |    if(draw_boundaries) {
113 |       glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
114 |       draw_circle2d(c0, rad0, 50); 
115 |       
116 |       //There's a bug, so draw one more(?)
117 |       draw_circle2d(c3, 0, 10);
118 |    }
119 | 
120 |    if(draw_particles) {
121 |       glColor3f(0,0,1);
122 |       glPointSize(3);
123 |       glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
124 |       draw_points2d(sim.particles);
125 |       for(unsigned int p = 0; p < sim.particles.size(); ++p) {
126 |          draw_circle2d(sim.particles[p], sim.particle_radius, 20);
127 |       }
128 |    }
129 | 
130 |    if(draw_velocities) {
131 |       glColor3f(1,0,0);
132 |       for(int j = 0;j < sim.nj; ++j) for(int i = 0; i < sim.ni; ++i) {
133 |          Vec2f pos((i+0.5f)*sim.dx,(j+0.5f)*sim.dx);
134 |          draw_arrow2d(pos, pos + 0.01f*sim.get_velocity(pos), 0.01f*sim.dx);
135 |       }
136 |    }
137 | 
138 | 
139 | }
140 | 
141 | void mouse(int button, int state, int x, int y)
142 | {
143 |    Vec2f newmouse;
144 |    cam.transform_mouse(x, y, newmouse.v);
145 |    //double newmousetime=get_time_in_seconds();
146 | 
147 |    oldmouse=newmouse;
148 |    //oldmousetime=newmousetime;
149 | }
150 | 
151 | void drag(int x, int y)
152 | {
153 |    Vec2f newmouse;
154 |    cam.transform_mouse(x, y, newmouse.v);
155 |    //double newmousetime=get_time_in_seconds();
156 | 
157 |    oldmouse=newmouse;
158 |    //oldmousetime=newmousetime;
159 | }
160 | 
161 | 
162 | void timer(int junk)
163 | {
164 | 
165 |    sim.advance(timestep);
166 | 
167 |    glutPostRedisplay();
168 |    glutTimerFunc(1, timer, 0);
169 | 
170 | }
171 | 
172 | 
173 | 
174 | 
175 | 
176 | 


--------------------------------------------------------------------------------
/openglutils.cpp:
--------------------------------------------------------------------------------
  1 | #include "openglutils.h"
  2 | 
  3 | #ifdef __APPLE__
  4 | #include <GLUT/glut.h> // why does Apple have to put glut.h here...
  5 | #else
  6 | #include <GL/glut.h> // ...when everyone else puts it here?
  7 | #endif
  8 | 
  9 | #include "vec.h"
 10 | #include <cfloat>
 11 | 
 12 | void draw_circle2d(const Vec2f& centre, float rad, int segs)
 13 | {
 14 |    glBegin(GL_POLYGON);
 15 |    for(int i=0;i<segs;i++){
 16 |       float cosine=rad*cos(i*2*M_PI/(float)(segs));
 17 |       float sine=rad* sin(i*2*M_PI/(float)(segs));
 18 |       glVertex2fv((Vec2f(cosine,sine) + centre).v);
 19 |    }
 20 |    glEnd();
 21 | }
 22 | 
 23 | void draw_grid2d(const Vec2f& origin, float dx, int nx, int ny) {
 24 |    float width = nx*dx;
 25 |    float height = ny*dx;
 26 |    
 27 |    glBegin(GL_LINES);
 28 |    for(int i = 0; i <= nx; i++) {
 29 |       Vec2f a(i*dx, 0);
 30 |       Vec2f b(i*dx, height);
 31 |       glVertex2fv((origin+a).v); 
 32 |       glVertex2fv((origin+b).v);
 33 |    }
 34 |    for(int j = 0; j <= ny; ++j) {
 35 |       Vec2f a(0,j*dx);
 36 |       Vec2f b(width,j*dx);
 37 |       glVertex2fv((origin + a).v); 
 38 |       glVertex2fv((origin + b).v);
 39 |    }
 40 |    glEnd();
 41 | }
 42 | 
 43 | void draw_box2d(const Vec2f& origin, float width, float height) {
 44 |    glBegin(GL_POLYGON);
 45 |    glVertex2fv(origin.v);
 46 |    glVertex2fv((origin + Vec2f(0, height)).v);
 47 |    glVertex2fv((origin + Vec2f(width, height)).v);
 48 |    glVertex2fv((origin + Vec2f(width, 0)).v);
 49 |    glEnd();
 50 | }
 51 | 
 52 | void draw_segmentset2d(const std::vector<Vec2f>& vertices, const std::vector<Vec2ui>& edges) {
 53 |    glBegin(GL_LINES);
 54 |    for(unsigned int i = 0; i < edges.size(); ++i) {
 55 |       glVertex2fv(vertices[edges[i][0]].v);      
 56 |       glVertex2fv(vertices[edges[i][1]].v);
 57 |    }
 58 |    glEnd();
 59 | }
 60 | 
 61 | void draw_segmentset2d(const std::vector<Vec2f>& vertices, const std::vector<Vec2i>& edges) {
 62 |    glBegin(GL_LINES);
 63 |    for(unsigned int i = 0; i < edges.size(); ++i) {
 64 |       glVertex2fv(vertices[edges[i][0]].v);      
 65 |       glVertex2fv(vertices[edges[i][1]].v);
 66 |    }
 67 |    glEnd();
 68 | }
 69 | 
 70 | void draw_points2d(const std::vector<Vec2f>& points) {
 71 |    glBegin(GL_POINTS);
 72 |    for(unsigned int i = 0; i < points.size(); ++i) {
 73 |       glVertex2fv(points[i].v);      
 74 |    }
 75 |    glEnd();
 76 | }
 77 | 
 78 | void draw_polygon2d(const std::vector<Vec2f>& vertices) {
 79 |    glBegin(GL_POLYGON);
 80 |    for(unsigned int i = 0; i < vertices.size(); ++i)
 81 |       glVertex2fv(vertices[i].v);      
 82 |    glEnd();
 83 | }
 84 | 
 85 | void draw_polygon2d(const std::vector<Vec2f>& vertices, const std::vector<int>& order) {
 86 |    glBegin(GL_POLYGON);
 87 |    for(unsigned int i = 0; i < order.size(); ++i)
 88 |       glVertex2fv(vertices[order[i]].v);      
 89 |    glEnd();
 90 | 
 91 | }
 92 | void draw_segment2d(const Vec2f& start, const Vec2f& end) {
 93 |    glBegin(GL_LINES);
 94 |    glVertex2fv(start.v);      
 95 |    glVertex2fv(end.v);      
 96 |    glEnd();
 97 | }
 98 | 
 99 | void draw_arrow2d(const Vec2f& start, const Vec2f& end, float arrow_head_len)
100 | {
101 |    Vec2f direction = end - start;
102 | 
103 |    Vec2f dir_norm = direction;
104 |    
105 |    //TODO Possibly automatically scale arrowhead length based on vector magnitude
106 |    if(mag(dir_norm) < 1e-14)
107 |       return;
108 | 
109 |    normalize(dir_norm);
110 |    Vec2f perp(dir_norm[1],-dir_norm[0]);
111 | 
112 |    Vec2f tip_left = end + arrow_head_len/(float)sqrt(2.0)*(-dir_norm + perp);
113 |    Vec2f tip_right = end + arrow_head_len/(float)sqrt(2.0)*(-dir_norm - perp);
114 |    
115 |    glBegin(GL_LINES);
116 |    glVertex2fv(start.v);
117 |    glVertex2fv(end.v);
118 |    glVertex2fv(end.v);
119 |    glVertex2fv(tip_left.v);
120 |    glVertex2fv(end.v);
121 |    glVertex2fv(tip_right.v);
122 |    glEnd();
123 |     
124 | }
125 | 
126 | void draw_trimesh2d(const std::vector<Vec2f>& vertices, const std::vector<Vec3ui>& tris) {
127 |    glBegin(GL_TRIANGLES);
128 |    for(unsigned int i = 0; i < tris.size(); ++i) {
129 |       glVertex2fv(vertices[tris[i][0]].v);
130 |       glVertex2fv(vertices[tris[i][1]].v);
131 |       glVertex2fv(vertices[tris[i][2]].v);
132 |     }
133 |    glEnd();
134 | }
135 |        
136 |        
137 | void hueToRGB(float hue, float sat, float val, float &r, float &g, float &b) {   
138 |    //compute hue (adapted from an older Wikipedia article)
139 |    int Hi = (int)(floor(hue / 60.0f)) % 6;
140 |    float f = hue / 60 - Hi;
141 |    float p = val * (1 - sat);
142 |    float q = val * (1- f * sat);
143 |    float t = val * (1 - (1 - f) * sat);
144 |    
145 |    switch(Hi) {
146 |       case 0:
147 |          r=val;
148 |          g=t;
149 |          b=p;
150 |          break;
151 |       case 1:
152 |          r=q;
153 |          g=val;
154 |          b=p;
155 |          break;
156 |       case 2:
157 |          r=p;
158 |          g=val;
159 |          b=t;
160 |          break;
161 |       case 3:
162 |          r=p;
163 |          g=q;
164 |          b=val;
165 |          break;
166 |       case 4:
167 |          r=t;
168 |          g=p;
169 |          b=val;
170 |         break;
171 |       case 5:
172 |          r=val;
173 |          g=p;
174 |          b=q;
175 |          break;
176 |    }
177 | }
178 | 
179 | void draw_grid_data2d(Array2f& data, Vec2f origin, float dx, bool color) {
180 |    float max_val = FLT_MIN;
181 |    float min_val = FLT_MAX;
182 |    for(int j = 0; j < data.nj; ++j) for(int i = 0; i < data.ni; ++i) {
183 |       max_val = max(data(i,j),max_val);
184 |       min_val = min(data(i,j),min_val);
185 |    }
186 |    
187 |    for(int j = 0; j < data.nj; ++j) {
188 |       for(int i = 0; i < data.ni; ++i) {
189 |          glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
190 |          Vec2f bl = origin + Vec2f(i*dx,j*dx);
191 |          float r,g,b;
192 |          if(color) {
193 |             hueToRGB(240*(data(i,j) - min_val)/(max_val-min_val), 1, 1, r,g,b);
194 |          }
195 |          else {
196 |             float gray = (data(i,j) - min_val)/(max_val-min_val);
197 |             r = g = b = gray;
198 |          }
199 |          //TODO Black body colormap, if I can find it.
200 |          glColor3f(r,g,b);
201 |          draw_box2d(bl, dx, dx);
202 |       }
203 |    }
204 | 
205 | }
206 | 
207 | void draw_trimesh3d(const std::vector<Vec3f>& vertices, const std::vector<Vec3ui>& tris) {
208 |    glBegin(GL_TRIANGLES);
209 |    for(unsigned int i = 0; i < tris.size(); ++i) {
210 |       glVertex3fv(vertices[tris[i][0]].v);
211 |       glVertex3fv(vertices[tris[i][1]].v);
212 |       glVertex3fv(vertices[tris[i][2]].v);
213 |    }
214 |    glEnd();
215 | }
216 | 
217 | void draw_trimesh3d(const std::vector<Vec3f>& vertices, const std::vector<Vec3ui>& tris, const std::vector<Vec3f> & normals) {
218 |    glBegin(GL_TRIANGLES);
219 |    for(unsigned int i = 0; i < tris.size(); ++i) {
220 |       glNormal3fv(normals[tris[i][0]].v);
221 |       glVertex3fv(vertices[tris[i][0]].v);
222 |       glNormal3fv(normals[tris[i][1]].v);
223 |       glVertex3fv(vertices[tris[i][1]].v);
224 |       glNormal3fv(normals[tris[i][2]].v);
225 |       glVertex3fv(vertices[tris[i][2]].v);
226 |    }
227 |    glEnd();
228 | }
229 | 
230 | void draw_box3d(const Vec3f& dimensions) {
231 |    
232 |    //Draw an axis-aligned box with specified dimensions, 
233 |    //where the midpoint of the box is at the origin
234 | 
235 |    float width = dimensions[0];
236 |    float height = dimensions[1];
237 |    float depth = dimensions[2];
238 | 
239 |    glBegin(GL_POLYGON);
240 |    glNormal3f(-1,0,0);
241 |    glVertex3f(-0.5*width, -0.5*height, 0.5*depth);
242 |    glVertex3f(-0.5*width, 0.5*height, 0.5*depth);
243 |    glVertex3f(-0.5*width, 0.5*height, -0.5*depth);
244 |    glVertex3f(-0.5*width, -0.5*height, -0.5*depth);
245 |    glEnd();
246 | 
247 |    glBegin(GL_POLYGON);
248 |    glNormal3f(1,0,0);
249 |    glVertex3f(0.5*width, -0.5*height, 0.5*depth);
250 |    glVertex3f(0.5*width, 0.5*height, 0.5*depth);
251 |    glVertex3f(0.5*width, 0.5*height, -0.5*depth);
252 |    glVertex3f(0.5*width, -0.5*height, -0.5*depth);
253 |    glEnd();
254 | 
255 |    glBegin(GL_POLYGON);
256 |    glNormal3f(0,0,-1);
257 |    glVertex3f(-0.5*width, -0.5*height, -0.5*depth);
258 |    glVertex3f(0.5*width, -0.5*height, -0.5*depth);
259 |    glVertex3f(0.5*width, 0.5*height, -0.5*depth);
260 |    glVertex3f(-0.5*width, 0.5*height, -0.5*depth);
261 |    glEnd();
262 | 
263 |    glBegin(GL_POLYGON);
264 |    glNormal3f(0,0,1);
265 |    glVertex3f(-0.5*width, -0.5*height, 0.5*depth);
266 |    glVertex3f(0.5*width, -0.5*height, 0.5*depth);
267 |    glVertex3f(0.5*width, 0.5*height, 0.5*depth);
268 |    glVertex3f(-0.5*width, 0.5*height, 0.5*depth);
269 |    glEnd();
270 | 
271 |    glBegin(GL_POLYGON);
272 |    glNormal3f(0,-1,0);
273 |    glVertex3f(-0.5*width, -0.5*height, 0.5*depth);
274 |    glVertex3f(0.5*width, -0.5*height, 0.5*depth);
275 |    glVertex3f(0.5*width, -0.5*height, -0.5*depth);
276 |    glVertex3f(-0.5*width, -0.5*height, -0.5*depth);
277 |    glEnd();
278 | 
279 |    glBegin(GL_POLYGON);
280 |    glNormal3f(0,1,0);
281 |    glVertex3f(-0.5*width, 0.5*height, 0.5*depth);
282 |    glVertex3f(0.5*width, 0.5*height, 0.5*depth);
283 |    glVertex3f(0.5*width, 0.5*height, -0.5*depth);
284 |    glVertex3f(-0.5*width, 0.5*height, -0.5*depth);
285 |    glEnd();
286 | }
287 | 


--------------------------------------------------------------------------------
/openglutils.h:
--------------------------------------------------------------------------------
 1 | #ifndef OPENGL_UTILS_H
 2 | #define OPENGL_UTILS_H
 3 | 
 4 | #include <vector>
 5 | #include "vec.h"
 6 | #include "array2.h"
 7 | 
 8 | void draw_circle2d(const Vec2f& centre, float rad, int segs);
 9 | void draw_grid2d(const Vec2f& origin, float dx, int nx, int ny);
10 | void draw_box2d(const Vec2f& origin, float width, float height);
11 | void draw_segmentset2d(const std::vector<Vec2f>& vertices, const std::vector<Vec2ui>& edges);
12 | void draw_segmentset2d(const std::vector<Vec2f>& vertices, const std::vector<Vec2i>& edges);
13 | void draw_points2d(const std::vector<Vec2f>& points);
14 | void draw_polygon2d(const std::vector<Vec2f>& vertices);
15 | void draw_polygon2d(const std::vector<Vec2f>& vertices, const std::vector<int>& order);
16 | void draw_segment2d(const Vec2f& start, const Vec2f& end);
17 | void draw_arrow2d(const Vec2f& start, const Vec2f& end, float arrow_head_len);
18 | void draw_grid_data2d(Array2f& data, Vec2f origin, float dx, bool color = false);
19 | void draw_trimesh2d(const std::vector<Vec2f>& vertices, const std::vector<Vec3ui>& tris);    
20 |    
21 | void draw_trimesh3d(const std::vector<Vec3f>& vertices, const std::vector<Vec3ui>& tris);
22 | void draw_trimesh3d(const std::vector<Vec3f>& vertices, const std::vector<Vec3ui>& tris, const std::vector<Vec3f>& normals);
23 | void draw_box3d(const Vec3f& dimensions);
24 | 
25 | #endif


--------------------------------------------------------------------------------
/pcgsolver/blas_wrapper.h:
--------------------------------------------------------------------------------
 1 | #ifndef BLAS_WRAPPER_H
 2 | #define BLAS_WRAPPER_H
 3 | 
 4 | // Simple placeholder code for BLAS calls - replace with calls to a real BLAS library
 5 | 
 6 | #include <vector>
 7 | 
 8 | namespace BLAS{
 9 | 
10 | // dot products ==============================================================
11 | 
12 | inline double dot(const std::vector<double> &x, const std::vector<double> &y)
13 | { 
14 |    //return cblas_ddot((int)x.size(), &x[0], 1, &y[0], 1); 
15 | 
16 |    double sum = 0;
17 |    for(unsigned int i = 0; i < x.size(); ++i)
18 |       sum += x[i]*y[i];
19 |    return sum;
20 | }
21 | 
22 | // inf-norm (maximum absolute value: index of max returned) ==================
23 | 
24 | inline int index_abs_max(const std::vector<double> &x)
25 | { 
26 |    //return cblas_idamax((int)x.size(), &x[0], 1); 
27 |    int maxind = 0;
28 |    double maxvalue = 0;
29 |    for(unsigned int i = 0; i < x.size(); ++i) {
30 |       if(fabs(x[i]) > maxvalue) {
31 |          maxvalue = fabs(x[i]);
32 |          maxind = i;
33 |       }
34 |    }
35 |    return maxind;
36 | }
37 | 
38 | // inf-norm (maximum absolute value) =========================================
39 | // technically not part of BLAS, but useful
40 | 
41 | inline double abs_max(const std::vector<double> &x)
42 | { return std::fabs(x[index_abs_max(x)]); }
43 | 
44 | // saxpy (y=alpha*x+y) =======================================================
45 | 
46 | inline void add_scaled(double alpha, const std::vector<double> &x, std::vector<double> &y)
47 | { 
48 |    //cblas_daxpy((int)x.size(), alpha, &x[0], 1, &y[0], 1); 
49 |    for(unsigned int i = 0; i < x.size(); ++i)
50 |       y[i] += alpha*x[i];
51 | }
52 | 
53 | }
54 | #endif
55 | 


--------------------------------------------------------------------------------
/pcgsolver/pcg_solver.h:
--------------------------------------------------------------------------------
  1 | #ifndef PCG_SOLVER_H
  2 | #define PCG_SOLVER_H
  3 | 
  4 | // Implements PCG with Modified Incomplete Cholesky (0) preconditioner.
  5 | // PCGSolver<T> is the main class for setting up and solving a linear system.
  6 | // Note that this only handles symmetric positive (semi-)definite matrices,
  7 | // with guarantees made only for M-matrices (where off-diagonal entries are all
  8 | // non-positive, and row sums are non-negative).
  9 | 
 10 | #include <cmath>
 11 | #include "sparse_matrix.h"
 12 | #include "blas_wrapper.h"
 13 | 
 14 | //============================================================================
 15 | // A simple compressed sparse column data structure (with separate diagonal)
 16 | // for lower triangular matrices
 17 | 
 18 | template<class T>
 19 | struct SparseColumnLowerFactor
 20 | {
 21 |    unsigned int n;
 22 |    std::vector<T> invdiag; // reciprocals of diagonal elements
 23 |    std::vector<T> value; // values below the diagonal, listed column by column
 24 |    std::vector<unsigned int> rowindex; // a list of all row indices, for each column in turn
 25 |    std::vector<unsigned int> colstart; // where each column begins in rowindex (plus an extra entry at the end, of #nonzeros)
 26 |    std::vector<T> adiag; // just used in factorization: minimum "safe" diagonal entry allowed
 27 | 
 28 |    explicit SparseColumnLowerFactor(unsigned int n_=0)
 29 |       : n(n_), invdiag(n_), colstart(n_+1), adiag(n_)
 30 |    {}
 31 | 
 32 |    void clear(void)
 33 |    {
 34 |       n=0;
 35 |       invdiag.clear();
 36 |       value.clear();
 37 |       rowindex.clear();
 38 |       colstart.clear();
 39 |       adiag.clear();
 40 |    }
 41 | 
 42 |    void resize(unsigned int n_)
 43 |    {
 44 |       n=n_;
 45 |       invdiag.resize(n);
 46 |       colstart.resize(n+1);
 47 |       adiag.resize(n);
 48 |    }
 49 | 
 50 |    void write_matlab(std::ostream &output, const char *variable_name)
 51 |    {
 52 |       output<<variable_name<<"=sparse([";
 53 |       for(unsigned int i=0; i<n; ++i){
 54 |          output<<" "<<i+1;
 55 |          for(unsigned int j=colstart[i]; j<colstart[i+1]; ++j){
 56 |             output<<" "<<rowindex[j]+1;
 57 |          }
 58 |       }
 59 |       output<<"],...\n  [";
 60 |       for(unsigned int i=0; i<n; ++i){
 61 |          output<<" "<<i+1;
 62 |          for(unsigned int j=colstart[i]; j<colstart[i+1]; ++j){
 63 |             output<<" "<<i+1;
 64 |          }
 65 |       }
 66 |       output<<"],...\n  [";
 67 |       for(unsigned int i=0; i<n; ++i){
 68 |          output<<" "<<(invdiag[i]!=0 ? 1/invdiag[i] : 0);
 69 |          for(unsigned int j=colstart[i]; j<colstart[i+1]; ++j){
 70 |             output<<" "<<value[j];
 71 |          }
 72 |       }
 73 |       output<<"], "<<n<<", "<<n<<");"<<std::endl;
 74 |    }
 75 | };
 76 | 
 77 | //============================================================================
 78 | // Incomplete Cholesky factorization, level zero, with option for modified version.
 79 | // Set modification_parameter between zero (regular incomplete Cholesky) and
 80 | // one (fully modified version), with values close to one usually giving the best
 81 | // results. The min_diagonal_ratio parameter is used to detect and correct
 82 | // problems in factorization: if a pivot is this much less than the diagonal
 83 | // entry from the original matrix, the original matrix entry is used instead.
 84 | 
 85 | template<class T>
 86 | void factor_modified_incomplete_cholesky0(const SparseMatrix<T> &matrix, SparseColumnLowerFactor<T> &factor,
 87 |                                           T modification_parameter=0.97, T min_diagonal_ratio=0.25)
 88 | {
 89 |    // first copy lower triangle of matrix into factor (Note: assuming A is symmetric of course!)
 90 |    factor.resize(matrix.n);
 91 |    zero(factor.invdiag); // important: eliminate old values from previous solves!
 92 |    factor.value.resize(0);
 93 |    factor.rowindex.resize(0);
 94 |    zero(factor.adiag);
 95 |    for(unsigned int i=0; i<matrix.n; ++i){
 96 |       factor.colstart[i]=(unsigned int)factor.rowindex.size();
 97 |       for(unsigned int j=0; j<matrix.index[i].size(); ++j){
 98 |          if(matrix.index[i][j]>i){
 99 |             factor.rowindex.push_back(matrix.index[i][j]);
100 |             factor.value.push_back(matrix.value[i][j]);
101 |          }else if(matrix.index[i][j]==i){
102 |             factor.invdiag[i]=factor.adiag[i]=matrix.value[i][j];
103 |          }
104 |       }
105 |    }
106 |    factor.colstart[matrix.n]=(unsigned int)factor.rowindex.size();
107 |    // now do the incomplete factorization (figure out numerical values)
108 | 
109 |    // MATLAB code:
110 |    // L=tril(A);
111 |    // for k=1:size(L,2)
112 |    //   L(k,k)=sqrt(L(k,k));
113 |    //   L(k+1:end,k)=L(k+1:end,k)/L(k,k);
114 |    //   for j=find(L(:,k))'
115 |    //     if j>k
116 |    //       fullupdate=L(:,k)*L(j,k);
117 |    //       incompleteupdate=fullupdate.*(A(:,j)~=0);
118 |    //       missing=sum(fullupdate-incompleteupdate);
119 |    //       L(j:end,j)=L(j:end,j)-incompleteupdate(j:end);
120 |    //       L(j,j)=L(j,j)-omega*missing;
121 |    //     end
122 |    //   end
123 |    // end
124 | 
125 |    for(unsigned int k=0; k<matrix.n; ++k){
126 |       if(factor.adiag[k]==0) continue; // null row/column
127 |       // figure out the final L(k,k) entry
128 |       if(factor.invdiag[k]<min_diagonal_ratio*factor.adiag[k])
129 |          factor.invdiag[k]=1/sqrt(factor.adiag[k]); // drop to Gauss-Seidel here if the pivot looks dangerously small
130 |       else
131 |          factor.invdiag[k]=1/sqrt(factor.invdiag[k]);
132 |       // finalize the k'th column L(:,k)
133 |       for(unsigned int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
134 |          factor.value[p]*=factor.invdiag[k];
135 |       }
136 |       // incompletely eliminate L(:,k) from future columns, modifying diagonals
137 |       for(unsigned int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
138 |          unsigned int j=factor.rowindex[p]; // work on column j
139 |          T multiplier=factor.value[p];
140 |          T missing=0;
141 |          unsigned int a=factor.colstart[k];
142 |          // first look for contributions to missing from dropped entries above the diagonal in column j
143 |          unsigned int b=0;
144 |          while(a<factor.colstart[k+1] && factor.rowindex[a]<j){
145 |             // look for factor.rowindex[a] in matrix.index[j] starting at b
146 |             while(b<matrix.index[j].size()){
147 |                if(matrix.index[j][b]<factor.rowindex[a])
148 |                   ++b;
149 |                else if(matrix.index[j][b]==factor.rowindex[a])
150 |                   break;
151 |                else{
152 |                   missing+=factor.value[a];
153 |                   break;
154 |                }
155 |             }
156 |             ++a;
157 |          }
158 |          // adjust the diagonal j,j entry
159 |          if(a<factor.colstart[k+1] && factor.rowindex[a]==j){
160 |             factor.invdiag[j]-=multiplier*factor.value[a];
161 |          }
162 |          ++a;
163 |          // and now eliminate from the nonzero entries below the diagonal in column j (or add to missing if we can't)
164 |          b=factor.colstart[j];
165 |          while(a<factor.colstart[k+1] && b<factor.colstart[j+1]){
166 |             if(factor.rowindex[b]<factor.rowindex[a])
167 |                ++b;
168 |             else if(factor.rowindex[b]==factor.rowindex[a]){
169 |                factor.value[b]-=multiplier*factor.value[a];
170 |                ++a;
171 |                ++b;
172 |             }else{
173 |                missing+=factor.value[a];
174 |                ++a;
175 |             }
176 |          }
177 |          // and if there's anything left to do, add it to missing
178 |          while(a<factor.colstart[k+1]){
179 |             missing+=factor.value[a];
180 |             ++a;
181 |          }
182 |          // and do the final diagonal adjustment from the missing entries
183 |          factor.invdiag[j]-=modification_parameter*multiplier*missing;
184 |       }
185 |    }
186 | }
187 | 
188 | //============================================================================
189 | // Solution routines with lower triangular matrix.
190 | 
191 | // solve L*result=rhs
192 | template<class T>
193 | void solve_lower(const SparseColumnLowerFactor<T> &factor, const std::vector<T> &rhs, std::vector<T> &result)
194 | {
195 |    assert(factor.n==rhs.size());
196 |    assert(factor.n==result.size());
197 |    result=rhs;
198 |    for(unsigned int i=0; i<factor.n; ++i){
199 |       result[i]*=factor.invdiag[i];
200 |       for(unsigned int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
201 |          result[factor.rowindex[j]]-=factor.value[j]*result[i];
202 |       }
203 |    }
204 | }
205 | 
206 | // solve L^T*result=rhs
207 | template<class T>
208 | void solve_lower_transpose_in_place(const SparseColumnLowerFactor<T> &factor, std::vector<T> &x)
209 | {
210 |    assert(factor.n==x.size());
211 |    assert(factor.n>0);
212 |    unsigned int i=factor.n;
213 |    do{
214 |       --i;
215 |       for(unsigned int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
216 |          x[i]-=factor.value[j]*x[factor.rowindex[j]];
217 |       }
218 |       x[i]*=factor.invdiag[i];
219 |    }while(i!=0);
220 | }
221 | 
222 | //============================================================================
223 | // Encapsulates the Conjugate Gradient algorithm with incomplete Cholesky
224 | // factorization preconditioner.
225 | 
226 | template <class T>
227 | struct PCGSolver
228 | {
229 |    PCGSolver(void)
230 |    {
231 |       set_solver_parameters(1e-5, 100, 0.97, 0.25);
232 |    }
233 | 
234 |    void set_solver_parameters(T tolerance_factor_, int max_iterations_, T modified_incomplete_cholesky_parameter_=0.97, T min_diagonal_ratio_=0.25)
235 |    {
236 |       tolerance_factor=tolerance_factor_;
237 |       if(tolerance_factor<1e-30) tolerance_factor=1e-30;
238 |       max_iterations=max_iterations_;
239 |       modified_incomplete_cholesky_parameter=modified_incomplete_cholesky_parameter_;
240 |       min_diagonal_ratio=min_diagonal_ratio_;
241 |    }
242 | 
243 |    bool solve(const SparseMatrix<T> &matrix, const std::vector<T> &rhs, std::vector<T> &result, T &residual_out, int &iterations_out) 
244 |    {
245 |       unsigned int n=matrix.n;
246 |       if(m.size()!=n){ m.resize(n); s.resize(n); z.resize(n); r.resize(n); }
247 |       zero(result);
248 |       r=rhs;
249 |       residual_out=BLAS::abs_max(r);
250 |       if(residual_out==0) {
251 |          iterations_out=0;
252 |          return true;
253 |       }
254 |       double tol=tolerance_factor*residual_out;
255 | 
256 |       form_preconditioner(matrix);
257 |       apply_preconditioner(r, z);
258 |       double rho=BLAS::dot(z, r);
259 |       if(rho==0 || rho!=rho) {
260 |          iterations_out=0;
261 |          return false;
262 |       }
263 | 
264 |       s=z;
265 |       fixed_matrix.construct_from_matrix(matrix);
266 |       int iteration;
267 |       for(iteration=0; iteration<max_iterations; ++iteration){
268 |          multiply(fixed_matrix, s, z);
269 |          double alpha=rho/BLAS::dot(s, z);
270 |          BLAS::add_scaled(alpha, s, result);
271 |          BLAS::add_scaled(-alpha, z, r);
272 |          residual_out=BLAS::abs_max(r);
273 |          if(residual_out<=tol) {
274 |             iterations_out=iteration+1;
275 |             return true; 
276 |          }
277 |          apply_preconditioner(r, z);
278 |          double rho_new=BLAS::dot(z, r);
279 |          double beta=rho_new/rho;
280 |          BLAS::add_scaled(beta, s, z); s.swap(z); // s=beta*s+z
281 |          rho=rho_new;
282 |       }
283 |       iterations_out=iteration;
284 |       return false;
285 |    }
286 | 
287 |    protected:
288 | 
289 |    // internal structures
290 |    SparseColumnLowerFactor<T> ic_factor; // modified incomplete cholesky factor
291 |    std::vector<T> m, z, s, r; // temporary vectors for PCG
292 |    FixedSparseMatrix<T> fixed_matrix; // used within loop
293 | 
294 |    // parameters
295 |    T tolerance_factor;
296 |    int max_iterations;
297 |    T modified_incomplete_cholesky_parameter;
298 |    T min_diagonal_ratio;
299 | 
300 |    void form_preconditioner(const SparseMatrix<T>& matrix)
301 |    {
302 |       factor_modified_incomplete_cholesky0(matrix, ic_factor);
303 |    }
304 | 
305 |    void apply_preconditioner(const std::vector<T> &x, std::vector<T> &result)
306 |    {
307 |       solve_lower(ic_factor, x, result);
308 |       solve_lower_transpose_in_place(ic_factor,result);
309 |    }
310 | };
311 | 
312 | #endif
313 | 


--------------------------------------------------------------------------------
/pcgsolver/sparse_matrix.h:
--------------------------------------------------------------------------------
  1 | #ifndef SPARSE_MATRIX_H
  2 | #define SPARSE_MATRIX_H
  3 | 
  4 | #include <iostream>
  5 | #include <vector>
  6 | #include "util.h"
  7 | 
  8 | //============================================================================
  9 | // Dynamic compressed sparse row matrix.
 10 | 
 11 | template<class T>
 12 | struct SparseMatrix
 13 | {
 14 |    unsigned int n; // dimension
 15 |    std::vector<std::vector<unsigned int> > index; // for each row, a list of all column indices (sorted)
 16 |    std::vector<std::vector<T> > value; // values corresponding to index
 17 | 
 18 |    explicit SparseMatrix(unsigned int n_=0, unsigned int expected_nonzeros_per_row=7)
 19 |       : n(n_), index(n_), value(n_)
 20 |    {
 21 |       for(unsigned int i=0; i<n; ++i){
 22 |          index[i].reserve(expected_nonzeros_per_row);
 23 |          value[i].reserve(expected_nonzeros_per_row);
 24 |       }
 25 |    }
 26 | 
 27 |    void clear(void)
 28 |    {
 29 |       n=0;
 30 |       index.clear();
 31 |       value.clear();
 32 |    }
 33 | 
 34 |    void zero(void)
 35 |    {
 36 |       for(unsigned int i=0; i<n; ++i){
 37 |          index[i].resize(0);
 38 |          value[i].resize(0);
 39 |       }
 40 |    }
 41 | 
 42 |    void resize(int n_)
 43 |    {
 44 |       n=n_;
 45 |       index.resize(n);
 46 |       value.resize(n);
 47 |    }
 48 | 
 49 |    T operator()(unsigned int i, unsigned int j) const
 50 |    {
 51 |       for(unsigned int k=0; k<index[i].size(); ++k){
 52 |          if(index[i][k]==j) return value[i][k];
 53 |          else if(index[i][k]>j) return 0;
 54 |       }
 55 |       return 0;
 56 |    }
 57 | 
 58 |    void set_element(unsigned int i, unsigned int j, T new_value)
 59 |    {
 60 |       unsigned int k=0;
 61 |       for(; k<index[i].size(); ++k){
 62 |          if(index[i][k]==j){
 63 |             value[i][k]=new_value;
 64 |             return;
 65 |          }else if(index[i][k]>j){
 66 |             insert(index[i], k, j);
 67 |             insert(value[i], k, new_value);
 68 |             return;
 69 |          }
 70 |       }
 71 |       index[i].push_back(j);
 72 |       value[i].push_back(new_value);
 73 |    }
 74 | 
 75 |    void add_to_element(unsigned int i, unsigned int j, T increment_value)
 76 |    {
 77 |       unsigned int k=0;
 78 |       for(; k<index[i].size(); ++k){
 79 |          if(index[i][k]==j){
 80 |             value[i][k]+=increment_value;
 81 |             return;
 82 |          }else if(index[i][k]>j){
 83 |             insert(index[i], k, j);
 84 |             insert(value[i], k, increment_value);
 85 |             return;
 86 |          }
 87 |       }
 88 |       index[i].push_back(j);
 89 |       value[i].push_back(increment_value);
 90 |    }
 91 | 
 92 |    // assumes indices is already sorted
 93 |    void add_sparse_row(unsigned int i, const std::vector<unsigned int> &indices, const std::vector<T> &values)
 94 |    {
 95 |       unsigned int j=0, k=0;
 96 |       while(j<indices.size() && k<index[i].size()){
 97 |          if(index[i][k]<indices[j]){
 98 |             ++k;
 99 |          }else if(index[i][k]>indices[j]){
100 |             insert(index[i], k, indices[j]);
101 |             insert(value[i], k, values[j]);
102 |             ++j;
103 |          }else{
104 |             value[i][k]+=values[j];
105 |             ++j;
106 |             ++k;
107 |          }
108 |       }
109 |       for(;j<indices.size(); ++j){
110 |          index[i].push_back(indices[j]);
111 |          value[i].push_back(values[j]);
112 |       }
113 |    }
114 | 
115 |    // assumes matrix has symmetric structure - so the indices in row i tell us which columns to delete i from
116 |    void symmetric_remove_row_and_column(unsigned int i)
117 |    {
118 |       for(unsigned int a=0; a<index[i].size(); ++a){
119 |          unsigned int j=index[i][a]; // 
120 |          for(unsigned int b=0; b<index[j].size(); ++b){
121 |             if(index[j][b]==i){
122 |                erase(index[j], b);
123 |                erase(value[j], b);
124 |                break;
125 |             }
126 |          }
127 |       }
128 |       index[i].resize(0);
129 |       value[i].resize(0);
130 |    }
131 | 
132 |    void write_matlab(std::ostream &output, const char *variable_name)
133 |    {
134 |       output<<variable_name<<"=sparse([";
135 |       for(unsigned int i=0; i<n; ++i){
136 |          for(unsigned int j=0; j<index[i].size(); ++j){
137 |             output<<i+1<<" ";
138 |          }
139 |       }
140 |       output<<"],...\n  [";
141 |       for(unsigned int i=0; i<n; ++i){
142 |          for(unsigned int j=0; j<index[i].size(); ++j){
143 |             output<<index[i][j]+1<<" ";
144 |          }
145 |       }
146 |       output<<"],...\n  [";
147 |       for(unsigned int i=0; i<n; ++i){
148 |          for(unsigned int j=0; j<value[i].size(); ++j){
149 |             output<<value[i][j]<<" ";
150 |          }
151 |       }
152 |       output<<"], "<<n<<", "<<n<<");"<<std::endl;
153 |    }
154 | };
155 | 
156 | typedef SparseMatrix<float> SparseMatrixf;
157 | typedef SparseMatrix<double> SparseMatrixd;
158 | 
159 | // perform result=matrix*x
160 | template<class T>
161 | void multiply(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
162 | {
163 |    assert(matrix.n==x.size());
164 |    result.resize(matrix.n);
165 |    for(unsigned int i=0; i<matrix.n; ++i){
166 |       result[i]=0;
167 |       for(unsigned int j=0; j<matrix.index[i].size(); ++j){
168 |          result[i]+=matrix.value[i][j]*x[matrix.index[i][j]];
169 |       }
170 |    }
171 | }
172 | 
173 | // perform result=result-matrix*x
174 | template<class T>
175 | void multiply_and_subtract(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
176 | {
177 |    assert(matrix.n==x.size());
178 |    result.resize(matrix.n);
179 |    for(unsigned int i=0; i<matrix.n; ++i){
180 |       for(unsigned int j=0; j<matrix.index[i].size(); ++j){
181 |          result[i]-=matrix.value[i][j]*x[matrix.index[i][j]];
182 |       }
183 |    }
184 | }
185 | 
186 | //============================================================================
187 | // Fixed version of SparseMatrix. This is not a good structure for dynamically
188 | // modifying the matrix, but can be significantly faster for matrix-vector
189 | // multiplies due to better data locality.
190 | 
191 | template<class T>
192 | struct FixedSparseMatrix
193 | {
194 |    unsigned int n; // dimension
195 |    std::vector<T> value; // nonzero values row by row
196 |    std::vector<unsigned int> colindex; // corresponding column indices
197 |    std::vector<unsigned int> rowstart; // where each row starts in value and colindex (and last entry is one past the end, the number of nonzeros)
198 | 
199 |    explicit FixedSparseMatrix(unsigned int n_=0)
200 |       : n(n_), value(0), colindex(0), rowstart(n_+1)
201 |    {}
202 | 
203 |    void clear(void)
204 |    {
205 |       n=0;
206 |       value.clear();
207 |       colindex.clear();
208 |       rowstart.clear();
209 |    }
210 | 
211 |    void resize(int n_)
212 |    {
213 |       n=n_;
214 |       rowstart.resize(n+1);
215 |    }
216 | 
217 |    void construct_from_matrix(const SparseMatrix<T> &matrix)
218 |    {
219 |       resize(matrix.n);
220 |       rowstart[0]=0;
221 |       for(unsigned int i=0; i<n; ++i){
222 |          rowstart[i+1]=rowstart[i]+matrix.index[i].size();
223 |       }
224 |       value.resize(rowstart[n]);
225 |       colindex.resize(rowstart[n]);
226 |       unsigned int j=0;
227 |       for(unsigned int i=0; i<n; ++i){
228 |          for(unsigned int k=0; k<matrix.index[i].size(); ++k){
229 |             value[j]=matrix.value[i][k];
230 |             colindex[j]=matrix.index[i][k];
231 |             ++j;
232 |          }
233 |       }
234 |    }
235 | 
236 |    void write_matlab(std::ostream &output, const char *variable_name)
237 |    {
238 |       output<<variable_name<<"=sparse([";
239 |       for(unsigned int i=0; i<n; ++i){
240 |          for(unsigned int j=rowstart[i]; j<rowstart[i+1]; ++j){
241 |             output<<i+1<<" ";
242 |          }
243 |       }
244 |       output<<"],...\n  [";
245 |       for(unsigned int i=0; i<n; ++i){
246 |          for(unsigned int j=rowstart[i]; j<rowstart[i+1]; ++j){
247 |             output<<colindex[j]+1<<" ";
248 |          }
249 |       }
250 |       output<<"],...\n  [";
251 |       for(unsigned int i=0; i<n; ++i){
252 |          for(unsigned int j=rowstart[i]; j<rowstart[i+1]; ++j){
253 |             output<<value[j]<<" ";
254 |          }
255 |       }
256 |       output<<"], "<<n<<", "<<n<<");"<<std::endl;
257 |    }
258 | };
259 | 
260 | typedef FixedSparseMatrix<float> FixedSparseMatrixf;
261 | typedef FixedSparseMatrix<double> FixedSparseMatrixd;
262 | 
263 | // perform result=matrix*x
264 | template<class T>
265 | void multiply(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
266 | {
267 |    assert(matrix.n==x.size());
268 |    result.resize(matrix.n);
269 |    for(unsigned int i=0; i<matrix.n; ++i){
270 |       result[i]=0;
271 |       for(unsigned int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
272 |          result[i]+=matrix.value[j]*x[matrix.colindex[j]];
273 |       }
274 |    }
275 | }
276 | 
277 | // perform result=result-matrix*x
278 | template<class T>
279 | void multiply_and_subtract(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
280 | {
281 |    assert(matrix.n==x.size());
282 |    result.resize(matrix.n);
283 |    for(unsigned int i=0; i<matrix.n; ++i){
284 |       for(unsigned int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
285 |          result[i]-=matrix.value[j]*x[matrix.colindex[j]];
286 |       }
287 |    }
288 | }
289 | 
290 | #endif
291 | 


--------------------------------------------------------------------------------
/util.h:
--------------------------------------------------------------------------------
  1 | #ifndef UTIL_H
  2 | #define UTIL_H
  3 | 
  4 | #include <algorithm>
  5 | #include <vector>
  6 | #include <cmath>
  7 | #include <iostream>
  8 | 
  9 | #ifndef M_PI
 10 | const double M_PI = 3.1415926535897932384626433832795;
 11 | #endif
 12 | 
 13 | #ifdef WIN32
 14 | #undef min
 15 | #undef max
 16 | #endif
 17 | 
 18 | using std::min;
 19 | using std::max;
 20 | using std::swap;
 21 | 
 22 | template<class T>
 23 | inline T sqr(const T& x)
 24 | { return x*x; }
 25 | 
 26 | template<class T>
 27 | inline T cube(const T& x)
 28 | { return x*x*x; }
 29 | 
 30 | template<class T>
 31 | inline T min(T a1, T a2, T a3)
 32 | { return min(a1, min(a2, a3)); }
 33 | 
 34 | template<class T>
 35 | inline T min(T a1, T a2, T a3, T a4)
 36 | { return min(min(a1, a2), min(a3, a4)); }
 37 | 
 38 | template<class T>
 39 | inline T min(T a1, T a2, T a3, T a4, T a5)
 40 | { return min(min(a1, a2), min(a3, a4), a5); }
 41 | 
 42 | template<class T>
 43 | inline T min(T a1, T a2, T a3, T a4, T a5, T a6)
 44 | { return min(min(a1, a2), min(a3, a4), min(a5, a6)); }
 45 | 
 46 | template<class T>
 47 | inline T max(T a1, T a2, T a3)
 48 | { return max(a1, max(a2, a3)); }
 49 | 
 50 | template<class T>
 51 | inline T max(T a1, T a2, T a3, T a4)
 52 | { return max(max(a1, a2), max(a3, a4)); }
 53 | 
 54 | template<class T>
 55 | inline T max(T a1, T a2, T a3, T a4, T a5)
 56 | { return max(max(a1, a2), max(a3, a4),  a5); }
 57 | 
 58 | template<class T>
 59 | inline T max(T a1, T a2, T a3, T a4, T a5, T a6)
 60 | { return max(max(a1, a2), max(a3, a4),  max(a5, a6)); }
 61 | 
 62 | template<class T>
 63 | inline void minmax(T a1, T a2, T& amin, T& amax)
 64 | {
 65 |    if(a1<a2){
 66 |       amin=a1;
 67 |       amax=a2;
 68 |    }else{
 69 |       amin=a2;
 70 |       amax=a1;
 71 |    }
 72 | }
 73 | 
 74 | template<class T>
 75 | inline void minmax(T a1, T a2, T a3, T& amin, T& amax)
 76 | {
 77 |    if(a1<a2){
 78 |       if(a1<a3){
 79 |          amin=a1;
 80 |          if(a2<a3) amax=a3;
 81 |          else amax=a2;
 82 |       }else{
 83 |          amin=a3;
 84 |          if(a1<a2) amax=a2;
 85 |          else amax=a1;
 86 |       }
 87 |    }else{
 88 |       if(a2<a3){
 89 |          amin=a2;
 90 |          if(a1<a3) amax=a3;
 91 |          else amax=a1;
 92 |       }else{
 93 |          amin=a3;
 94 |          amax=a1;
 95 |       }
 96 |    }
 97 | }
 98 | 
 99 | template<class T>
100 | inline void minmax(T a1, T a2, T a3, T a4, T& amin, T& amax)
101 | {
102 |    if(a1<a2){
103 |       if(a3<a4){
104 |          amin=min(a1,a3);
105 |          amax=max(a2,a4);
106 |       }else{
107 |          amin=min(a1,a4);
108 |          amax=max(a2,a3);
109 |       }
110 |    }else{
111 |       if(a3<a4){
112 |          amin=min(a2,a3);
113 |          amax=max(a1,a4);
114 |       }else{
115 |          amin=min(a2,a4);
116 |          amax=max(a1,a3);
117 |       }
118 |    }
119 | }
120 | 
121 | template<class T>
122 | inline void minmax(T a1, T a2, T a3, T a4, T a5, T& amin, T& amax)
123 | {
124 |    //@@@ the logic could be shortcircuited a lot!
125 |    amin=min(a1,a2,a3,a4,a5);
126 |    amax=max(a1,a2,a3,a4,a5);
127 | }
128 | 
129 | template<class T>
130 | inline void minmax(T a1, T a2, T a3, T a4, T a5, T a6, T& amin, T& amax)
131 | {
132 |    //@@@ the logic could be shortcircuited a lot!
133 |    amin=min(a1,a2,a3,a4,a5,a6);
134 |    amax=max(a1,a2,a3,a4,a5,a6);
135 | }
136 | 
137 | template<class T>
138 | inline void update_minmax(T a1, T& amin, T& amax)
139 | {
140 |    if(a1<amin) amin=a1;
141 |    else if(a1>amax) amax=a1;
142 | }
143 | 
144 | template<class T>
145 | inline void sort(T &a, T &b, T &c)
146 | {
147 |    T temp;
148 |    if(a<b){
149 |       if(a<c){
150 |     if(c<b){ // a<c<b
151 |             temp=c;c=b;b=temp;
152 |     } // else: a<b<c
153 |       }else{ // c<a<b
154 |     temp=c;c=b;b=a;a=temp;
155 |       }
156 |    }else{
157 |       if(b<c){
158 |     if(a<c){ //b<a<c
159 |        temp=b;b=a;a=temp;
160 |     }else{ // b<c<a
161 |        temp=b;b=c;c=a;a=temp;
162 |     }
163 |       }else{ // c<b<a
164 |     temp=c;c=a;a=temp;
165 |       }
166 |    }
167 | }
168 | 
169 | template<class T>
170 | inline T clamp(T a, T lower, T upper)
171 | {
172 |    if(a<lower) return lower;
173 |    else if(a>upper) return upper;
174 |    else return a;
175 | }
176 | 
177 | // only makes sense with T=float or double
178 | template<class T>
179 | inline T smooth_step(T r)
180 | {
181 |    if(r<0) return 0;
182 |    else if(r>1) return 1;
183 |    return r*r*r*(10+r*(-15+r*6));
184 | }
185 | 
186 | // only makes sense with T=float or double
187 | template<class T>
188 | inline T smooth_step(T r, T r_lower, T r_upper, T value_lower, T value_upper)
189 | { return value_lower + smooth_step((r-r_lower)/(r_upper-r_lower)) * (value_upper-value_lower); }
190 | 
191 | // only makes sense with T=float or double
192 | template<class T>
193 | inline T ramp(T r)
194 | { return smooth_step((r+1)/2)*2-1; }
195 | 
196 | #ifdef WIN32
197 | inline int lround(double x)
198 | {
199 |    if(x>0)
200 |       return (x-floor(x)<0.5) ? (int)floor(x) : (int)ceil(x);
201 |    else
202 |       return (x-floor(x)<=0.5) ? (int)floor(x) : (int)ceil(x);
203 | }
204 | 
205 | inline double remainder(double x, double y)
206 | {
207 |    return x-std::floor(x/y+0.5)*y;
208 | }
209 | #endif
210 | 
211 | inline unsigned int round_up_to_power_of_two(unsigned int n)
212 | {
213 |    int exponent=0;
214 |    --n;
215 |    while(n){
216 |       ++exponent;
217 |       n>>=1;
218 |    }
219 |    return 1<<exponent;
220 | }
221 | 
222 | inline unsigned int round_down_to_power_of_two(unsigned int n)
223 | {
224 |    int exponent=0;
225 |    while(n>1){
226 |       ++exponent;
227 |       n>>=1;
228 |    }
229 |    return 1<<exponent;
230 | }
231 | 
232 | // Transforms even the sequence 0,1,2,3,... into reasonably good random numbers 
233 | // Challenge: improve on this in speed and "randomness"!
234 | // This seems to pass several statistical tests, and is a bijective map (of 32-bit unsigned ints)
235 | inline unsigned int randhash(unsigned int seed)
236 | {
237 |    unsigned int i=(seed^0xA3C59AC3u)*2654435769u;
238 |    i^=(i>>16);
239 |    i*=2654435769u;
240 |    i^=(i>>16);
241 |    i*=2654435769u;
242 |    return i;
243 | }
244 | 
245 | // the inverse of randhash
246 | inline unsigned int unhash(unsigned int h)
247 | {
248 |    h*=340573321u;
249 |    h^=(h>>16);
250 |    h*=340573321u;
251 |    h^=(h>>16);
252 |    h*=340573321u;
253 |    h^=0xA3C59AC3u;
254 |    return h;
255 | }
256 | 
257 | // returns repeatable stateless pseudo-random number in [0,1]
258 | inline double randhashd(unsigned int seed)
259 | { return randhash(seed)/(double)UINT_MAX; }
260 | inline float randhashf(unsigned int seed)
261 | { return randhash(seed)/(float)UINT_MAX; }
262 | 
263 | // returns repeatable stateless pseudo-random number in [a,b]
264 | inline double randhashd(unsigned int seed, double a, double b)
265 | { return (b-a)*randhash(seed)/(double)UINT_MAX + a; }
266 | inline float randhashf(unsigned int seed, float a, float b)
267 | { return ( (b-a)*randhash(seed)/(float)UINT_MAX + a); }
268 | 
269 | inline int intlog2(int x)
270 | {
271 |    int exp=-1;
272 |    while(x){
273 |       x>>=1;
274 |       ++exp;
275 |    }
276 |    return exp;
277 | }
278 | 
279 | template<class T>
280 | inline void get_barycentric(T x, int& i, T& f, int i_low, int i_high)
281 | {
282 |    T s=std::floor(x);
283 |    i=(int)s;
284 |    if(i<i_low){
285 |       i=i_low;
286 |       f=0;
287 |    }else if(i>i_high-2){
288 |       i=i_high-2;
289 |       f=1;
290 |    }else
291 |       f=(T)(x-s);
292 | }
293 | 
294 | template<class S, class T>
295 | inline S lerp(const S& value0, const S& value1, T f)
296 | { return (1-f)*value0 + f*value1; }
297 | 
298 | template<class S, class T>
299 | inline S bilerp(const S& v00, const S& v10, 
300 |                 const S& v01, const S& v11, 
301 |                 T fx, T fy)
302 | { 
303 |    return lerp(lerp(v00, v10, fx),
304 |                lerp(v01, v11, fx), 
305 |                fy);
306 | }
307 | 
308 | template<class S, class T>
309 | inline S trilerp(const S& v000, const S& v100,
310 |                  const S& v010, const S& v110,
311 |                  const S& v001, const S& v101,  
312 |                  const S& v011, const S& v111,
313 |                  T fx, T fy, T fz) 
314 | {
315 |    return lerp(bilerp(v000, v100, v010, v110, fx, fy),
316 |                bilerp(v001, v101, v011, v111, fx, fy),
317 |                fz);
318 | }
319 | 
320 | template<class S, class T>
321 | inline S quadlerp(const S& v0000, const S& v1000,
322 |                   const S& v0100, const S& v1100,
323 |                   const S& v0010, const S& v1010,  
324 |                   const S& v0110, const S& v1110,
325 |                   const S& v0001, const S& v1001,
326 |                   const S& v0101, const S& v1101,
327 |                   const S& v0011, const S& v1011,  
328 |                   const S& v0111, const S& v1111,
329 |                   T fx, T fy, T fz, T ft) 
330 | {
331 |    return lerp(trilerp(v0000, v1000, v0100, v1100, v0010, v1010, v0110, v1110, fx, fy, fz),
332 |                trilerp(v0001, v1001, v0101, v1101, v0011, v1011, v0111, v1111, fx, fy, fz),
333 |                ft);
334 | }
335 | 
336 | // f should be between 0 and 1, with f=0.5 corresponding to balanced weighting between w0 and w2
337 | template<class T>
338 | inline void quadratic_bspline_weights(T f, T& w0, T& w1, T& w2)
339 | {
340 |    w0=T(0.5)*sqr(f-1);
341 |    w1=T(0.75)-sqr(f-T(0.5));;
342 |    w2=T(0.5)*sqr(f);
343 | }
344 | 
345 | // f should be between 0 and 1
346 | template<class T>
347 | inline void cubic_interp_weights(T f, T& wneg1, T& w0, T& w1, T& w2)
348 | {
349 |    T f2(f*f), f3(f2*f);
350 |    wneg1=-T(1./3)*f+T(1./2)*f2-T(1./6)*f3;
351 |    w0=1-f2+T(1./2)*(f3-f);
352 |    w1=f+T(1./2)*(f2-f3);
353 |    w2=T(1./6)*(f3-f);
354 | }
355 | 
356 | template<class S, class T>
357 | inline S cubic_interp(const S& value_neg1, const S& value0, const S& value1, const S& value2, T f)
358 | {
359 |    T wneg1, w0, w1, w2;
360 |    cubic_interp_weights(f, wneg1, w0, w1, w2);
361 |    return wneg1*value_neg1 + w0*value0 + w1*value1 + w2*value2;
362 | }
363 | 
364 | template<class T>
365 | void zero(std::vector<T>& v)
366 | { for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; }
367 | 
368 | template<class T>
369 | T abs_max(const std::vector<T>& v)
370 | {
371 |    T m=0;
372 |    for(int i=(int)v.size()-1; i>=0; --i){
373 |       if(std::fabs(v[i])>m)
374 |          m=std::fabs(v[i]);
375 |    }
376 |    return m;
377 | }
378 | 
379 | template<class T>
380 | bool contains(const std::vector<T>& a, T e)
381 | {
382 |    for(unsigned int i=0; i<a.size(); ++i)
383 |       if(a[i]==e) return true;
384 |    return false;
385 | }
386 | 
387 | template<class T>
388 | void add_unique(std::vector<T>& a, T e)
389 | {
390 |    for(unsigned int i=0; i<a.size(); ++i)
391 |       if(a[i]==e) return;
392 |    a.push_back(e);
393 | }
394 | 
395 | template<class T>
396 | void insert(std::vector<T>& a, unsigned int index, T e)
397 | {
398 |    a.push_back(a.back());
399 |    for(unsigned int i=(unsigned int)a.size()-1; i>index; --i)
400 |       a[i]=a[i-1];
401 |    a[index]=e;
402 | }
403 | 
404 | template<class T>
405 | void erase(std::vector<T>& a, unsigned int index)
406 | {
407 |    for(unsigned int i=index; i<a.size()-1; ++i)
408 |       a[i]=a[i+1];
409 |    a.pop_back();
410 | }
411 | 
412 | template<class T>
413 | void erase_swap(std::vector<T>& a, unsigned int index)
414 | {
415 |    for(unsigned int i=index; i<a.size()-1; ++i)
416 |       swap(a[i], a[i+1]);
417 |    a.pop_back();
418 | }
419 | 
420 | template<class T>
421 | void erase_unordered(std::vector<T>& a, unsigned int index)
422 | {
423 |    a[index]=a.back();
424 |    a.pop_back();
425 | }
426 | 
427 | template<class T>
428 | void erase_unordered_swap(std::vector<T>& a, unsigned int index)
429 | {
430 |    swap(a[index], a.back());
431 |    a.pop_back();
432 | }
433 | 
434 | template<class T>
435 | void find_and_erase_unordered(std::vector<T>& a, const T& doomed_element)
436 | {
437 |    for(unsigned int i=0; i<a.size(); ++i)
438 |       if(a[i]==doomed_element){
439 |          erase_unordered(a, i);
440 |          return;
441 |       }
442 | }
443 | 
444 | template<class T>
445 | void replace_once(std::vector<T>& a, const T& old_element, const T& new_element)
446 | {
447 |    for(unsigned int i=0; i<a.size(); ++i)
448 |       if(a[i]==old_element){
449 |          a[i]=new_element;
450 |          return;
451 |       }
452 | }
453 | 
454 | template<class T>
455 | void write_matlab(std::ostream& output, const std::vector<T>& a, const char *variable_name, bool column_vector=true, int significant_digits=18)
456 | {
457 |    output<<variable_name<<"=[";
458 |    std::streamsize old_precision=output.precision();
459 |    output.precision(significant_digits);
460 |    for(unsigned int i=0; i<a.size(); ++i){
461 |       output<<a[i]<<" ";
462 |    }
463 |    output<<"]";
464 |    if(column_vector)
465 |       output<<"'";
466 |    output<<";"<<std::endl;
467 |    output.precision(old_precision);
468 | }
469 | 
470 | #endif
471 | 


--------------------------------------------------------------------------------
/vec.h:
--------------------------------------------------------------------------------
  1 | #ifndef VEC_H
  2 | #define VEC_H
  3 | 
  4 | #include <cassert>
  5 | #include <cmath>
  6 | #include <iostream>
  7 | #include "util.h"
  8 | 
  9 | // Defines a thin wrapper around fixed size C-style arrays, using template parameters,
 10 | // which is useful for dealing with vectors of different dimensions.
 11 | // For example, float[3] is equivalent to Vec<3,float>.
 12 | // Entries in the vector are accessed with the overloaded [] operator, so
 13 | // for example if x is a Vec<3,float>, then the middle entry is x[1].
 14 | // For convenience, there are a number of typedefs for abbreviation:
 15 | //   Vec<3,float> -> Vec3f
 16 | //   Vec<2,int>   -> Vec2i
 17 | // and so on.
 18 | // Arithmetic operators are appropriately overloaded, and functions are defined
 19 | // for additional operations (such as dot-products, norms, cross-products, etc.)
 20 | 
 21 | template<unsigned int N, class T>
 22 | struct Vec
 23 | {
 24 |    T v[N];
 25 | 
 26 |    Vec<N,T>(void)
 27 |    {}
 28 | 
 29 |    Vec<N,T>(T value_for_all)
 30 |    { for(unsigned int i=0; i<N; ++i) v[i]=value_for_all; }
 31 | 
 32 |    template<class S>
 33 |    Vec<N,T>(const S *source)
 34 |    { for(unsigned int i=0; i<N; ++i) v[i]=(T)source[i]; }
 35 | 
 36 |    template <class S>
 37 |    explicit Vec<N,T>(const Vec<N,S>& source)
 38 |    { for(unsigned int i=0; i<N; ++i) v[i]=(T)source[i]; }
 39 | 
 40 |    Vec<N,T>(T v0, T v1)
 41 |    {
 42 |       assert(N==2);
 43 |       v[0]=v0; v[1]=v1;
 44 |    }
 45 | 
 46 |    Vec<N,T>(T v0, T v1, T v2)
 47 |    {
 48 |       assert(N==3);
 49 |       v[0]=v0; v[1]=v1; v[2]=v2;
 50 |    }
 51 | 
 52 |    Vec<N,T>(T v0, T v1, T v2, T v3)
 53 |    {
 54 |       assert(N==4);
 55 |       v[0]=v0; v[1]=v1; v[2]=v2; v[3]=v3;
 56 |    }
 57 | 
 58 |    Vec<N,T>(T v0, T v1, T v2, T v3, T v4)
 59 |    {
 60 |       assert(N==5);
 61 |       v[0]=v0; v[1]=v1; v[2]=v2; v[3]=v3; v[4]=v4;
 62 |    }
 63 | 
 64 |      Vec<N,T>(T v0, T v1, T v2, T v3, T v4, T v5)
 65 |    {
 66 |       assert(N==6);
 67 |       v[0]=v0; v[1]=v1; v[2]=v2; v[3]=v3; v[4]=v4; v[5]=v5;
 68 |    }
 69 | 
 70 |    T &operator[](int index)
 71 |    {
 72 |       assert(0<=index && (unsigned int)index<N);
 73 |       return v[index];
 74 |    }
 75 | 
 76 |    const T &operator[](int index) const
 77 |    {
 78 |       assert(0<=index && (unsigned int)index<N);
 79 |       return v[index];
 80 |    }
 81 | 
 82 |    operator bool(void) const
 83 |    {
 84 |       for(unsigned int i=0; i<N; ++i) if(v[i]) return true;
 85 |       return false;
 86 |    }
 87 | 
 88 |    Vec<N,T> operator+=(const Vec<N,T> &w)
 89 |    {
 90 |       for(unsigned int i=0; i<N; ++i) v[i]+=w[i];
 91 |       return *this;
 92 |    }
 93 | 
 94 |    Vec<N,T> operator+(const Vec<N,T> &w) const
 95 |    {
 96 |       Vec<N,T> sum(*this);
 97 |       sum+=w;
 98 |       return sum;
 99 |    }
100 | 
101 |    Vec<N,T> operator-=(const Vec<N,T> &w)
102 |    {
103 |       for(unsigned int i=0; i<N; ++i) v[i]-=w[i];
104 |       return *this;
105 |    }
106 | 
107 |    Vec<N,T> operator-(void) const // unary minus
108 |    {
109 |       Vec<N,T> negative;
110 |       for(unsigned int i=0; i<N; ++i) negative.v[i]=-v[i];
111 |       return negative;
112 |    }
113 | 
114 |    Vec<N,T> operator-(const Vec<N,T> &w) const // (binary) subtraction
115 |    {
116 |       Vec<N,T> diff(*this);
117 |       diff-=w;
118 |       return diff;
119 |    }
120 | 
121 |    Vec<N,T> operator*=(T a)
122 |    {
123 |       for(unsigned int i=0; i<N; ++i) v[i]*=a;
124 |       return *this;
125 |    }
126 | 
127 |    Vec<N,T> operator*(T a) const
128 |    {
129 |       Vec<N,T> w(*this);
130 |       w*=a;
131 |       return w;
132 |    }
133 | 
134 |    Vec<N,T> operator*(const Vec<N,T> &w) const
135 |    {
136 |       Vec<N,T> componentwise_product;
137 |       for(unsigned int i=0; i<N; ++i) componentwise_product[i]=v[i]*w.v[i];
138 |       return componentwise_product;
139 |    }
140 | 
141 |    Vec<N,T> operator/=(T a)
142 |    {
143 |       for(unsigned int i=0; i<N; ++i) v[i]/=a;
144 |       return *this;
145 |    }
146 | 
147 |    Vec<N,T> operator/(T a) const
148 |    {
149 |       Vec<N,T> w(*this);
150 |       w/=a;
151 |       return w;
152 |    }
153 | };
154 | 
155 | typedef Vec<2,double>         Vec2d;
156 | typedef Vec<2,float>          Vec2f;
157 | typedef Vec<2,int>            Vec2i;
158 | typedef Vec<2,unsigned int>   Vec2ui;
159 | typedef Vec<2,short>          Vec2s;
160 | typedef Vec<2,unsigned short> Vec2us;
161 | typedef Vec<2,char>           Vec2c;
162 | typedef Vec<2,unsigned char>  Vec2uc;
163 | 
164 | typedef Vec<3,double>         Vec3d;
165 | typedef Vec<3,float>          Vec3f;
166 | typedef Vec<3,int>            Vec3i;
167 | typedef Vec<3,unsigned int>   Vec3ui;
168 | typedef Vec<3,short>          Vec3s;
169 | typedef Vec<3,unsigned short> Vec3us;
170 | typedef Vec<3,char>           Vec3c;
171 | typedef Vec<3,unsigned char>  Vec3uc;
172 | 
173 | typedef Vec<4,double>         Vec4d;
174 | typedef Vec<4,float>          Vec4f;
175 | typedef Vec<4,int>            Vec4i;
176 | typedef Vec<4,unsigned int>   Vec4ui;
177 | typedef Vec<4,short>          Vec4s;
178 | typedef Vec<4,unsigned short> Vec4us;
179 | typedef Vec<4,char>           Vec4c;
180 | typedef Vec<4,unsigned char>  Vec4uc;
181 | 
182 | typedef Vec<6,double>         Vec6d;
183 | typedef Vec<6,float>          Vec6f;
184 | typedef Vec<6,unsigned int>   Vec6ui;
185 | typedef Vec<6,int>            Vec6i;
186 | typedef Vec<6,short>          Vec6s;
187 | typedef Vec<6,unsigned short> Vec6us;
188 | typedef Vec<6,char>           Vec6c;
189 | typedef Vec<6,unsigned char>  Vec6uc;
190 | 
191 | 
192 | template<unsigned int N, class T>
193 | T mag2(const Vec<N,T> &a)
194 | {
195 |    T l=sqr(a.v[0]);
196 |    for(unsigned int i=1; i<N; ++i) l+=sqr(a.v[i]);
197 |    return l;
198 | }
199 | 
200 | template<unsigned int N, class T>
201 | T mag(const Vec<N,T> &a)
202 | { return sqrt(mag2(a)); }
203 | 
204 | template<unsigned int N, class T> 
205 | inline T dist2(const Vec<N,T> &a, const Vec<N,T> &b)
206 | { 
207 |    T d=sqr(a.v[0]-b.v[0]);
208 |    for(unsigned int i=1; i<N; ++i) d+=sqr(a.v[i]-b.v[i]);
209 |    return d;
210 | }
211 | 
212 | template<unsigned int N, class T> 
213 | inline T dist(const Vec<N,T> &a, const Vec<N,T> &b)
214 | { return std::sqrt(dist2(a,b)); }
215 | 
216 | template<unsigned int N, class T> 
217 | inline void normalize(Vec<N,T> &a)
218 | { a/=mag(a); }
219 | 
220 | template<unsigned int N, class T> 
221 | inline Vec<N,T> normalized(const Vec<N,T> &a)
222 | { return a/mag(a); }
223 | 
224 | template<unsigned int N, class T> 
225 | inline T infnorm(const Vec<N,T> &a)
226 | {
227 |    T d=std::fabs(a.v[0]);
228 |    for(unsigned int i=1; i<N; ++i) d=max(std::fabs(a.v[i]),d);
229 |    return d;
230 | }
231 | 
232 | template<unsigned int N, class T>
233 | void zero(Vec<N,T> &a)
234 | { 
235 |    for(unsigned int i=0; i<N; ++i)
236 |       a.v[i] = 0;
237 | }
238 | 
239 | template<unsigned int N, class T>
240 | std::ostream &operator<<(std::ostream &out, const Vec<N,T> &v)
241 | {
242 |    out<<v.v[0];
243 |    for(unsigned int i=1; i<N; ++i)
244 |       out<<' '<<v.v[i];
245 |    return out;
246 | }
247 | 
248 | template<unsigned int N, class T>
249 | std::istream &operator>>(std::istream &in, Vec<N,T> &v)
250 | {
251 |    in>>v.v[0];
252 |    for(unsigned int i=1; i<N; ++i)
253 |       in>>v.v[i];
254 |    return in;
255 | }
256 | 
257 | template<unsigned int N, class T> 
258 | inline bool operator==(const Vec<N,T> &a, const Vec<N,T> &b)
259 | { 
260 |    bool t = (a.v[0] == b.v[0]);
261 |    unsigned int i=1;
262 |    while(i<N && t) {
263 |       t = t && (a.v[i]==b.v[i]); 
264 |       ++i;
265 |    }
266 |    return t;
267 | }
268 | 
269 | template<unsigned int N, class T> 
270 | inline bool operator!=(const Vec<N,T> &a, const Vec<N,T> &b)
271 | { 
272 |    bool t = (a.v[0] != b.v[0]);
273 |    unsigned int i=1;
274 |    while(i<N && !t) {
275 |       t = t || (a.v[i]!=b.v[i]); 
276 |       ++i;
277 |    }
278 |    return t;
279 | }
280 | 
281 | template<unsigned int N, class T>
282 | inline Vec<N,T> operator*(T a, const Vec<N,T> &v)
283 | {
284 |    Vec<N,T> w(v);
285 |    w*=a;
286 |    return w;
287 | }
288 | 
289 | template<unsigned int N, class T>
290 | inline T min(const Vec<N,T> &a)
291 | {
292 |    T m=a.v[0];
293 |    for(unsigned int i=1; i<N; ++i) if(a.v[i]<m) m=a.v[i];
294 |    return m;
295 | }
296 | 
297 | template<unsigned int N, class T>
298 | inline Vec<N,T> min_union(const Vec<N,T> &a, const Vec<N,T> &b)
299 | {
300 |    Vec<N,T> m;
301 |    for(unsigned int i=0; i<N; ++i) (a.v[i] < b.v[i]) ? m.v[i]=a.v[i] : m.v[i]=b.v[i];
302 |    return m;
303 | }
304 | 
305 | template<unsigned int N, class T>
306 | inline Vec<N,T> max_union(const Vec<N,T> &a, const Vec<N,T> &b)
307 | {
308 |    Vec<N,T> m;
309 |    for(unsigned int i=0; i<N; ++i) (a.v[i] > b.v[i]) ? m.v[i]=a.v[i] : m.v[i]=b.v[i];
310 |    return m;
311 | }
312 | 
313 | template<unsigned int N, class T>
314 | inline T max(const Vec<N,T> &a)
315 | {
316 |    T m=a.v[0];
317 |    for(unsigned int i=1; i<N; ++i) if(a.v[i]>m) m=a.v[i];
318 |    return m;
319 | }
320 | 
321 | template<unsigned int N, class T>
322 | inline T dot(const Vec<N,T> &a, const Vec<N,T> &b)
323 | {
324 |    T d=a.v[0]*b.v[0];
325 |    for(unsigned int i=1; i<N; ++i) d+=a.v[i]*b.v[i];
326 |    return d;
327 | }
328 | 
329 | template<class T> 
330 | inline Vec<2,T> rotate(const Vec<2,T>& a, float angle) 
331 | {
332 |    T c = cos(angle);
333 |    T s = sin(angle);
334 |    return Vec<2,T>(c*a[0] - s*a[1],s*a[0] + c*a[1]); // counter-clockwise rotation
335 | }
336 | 
337 | template<class T>
338 | inline Vec<2,T> perp(const Vec<2,T> &a)
339 | { return Vec<2,T>(-a.v[1], a.v[0]); } // counter-clockwise rotation by 90 degrees
340 | 
341 | template<class T>
342 | inline T cross(const Vec<2,T> &a, const Vec<2,T> &b)
343 | { return a.v[0]*b.v[1]-a.v[1]*b.v[0]; }
344 | 
345 | template<class T>
346 | inline Vec<3,T> cross(const Vec<3,T> &a, const Vec<3,T> &b)
347 | { return Vec<3,T>(a.v[1]*b.v[2]-a.v[2]*b.v[1], a.v[2]*b.v[0]-a.v[0]*b.v[2], a.v[0]*b.v[1]-a.v[1]*b.v[0]); }
348 | 
349 | template<class T>
350 | inline T triple(const Vec<3,T> &a, const Vec<3,T> &b, const Vec<3,T> &c)
351 | { return a.v[0]*(b.v[1]*c.v[2]-b.v[2]*c.v[1])
352 |         +a.v[1]*(b.v[2]*c.v[0]-b.v[0]*c.v[2])
353 |         +a.v[2]*(b.v[0]*c.v[1]-b.v[1]*c.v[0]); }
354 | 
355 | template<unsigned int N, class T>
356 | inline unsigned int hash(const Vec<N,T> &a)
357 | {
358 |    unsigned int h=a.v[0];
359 |    for(unsigned int i=1; i<N; ++i)
360 |       h=hash(h ^ a.v[i]);
361 |    return h;
362 | }
363 | 
364 | template<unsigned int N, class T>
365 | inline void assign(const Vec<N,T> &a, T &a0, T &a1)
366 | { 
367 |    assert(N==2);
368 |    a0=a.v[0]; a1=a.v[1];
369 | }
370 | 
371 | template<unsigned int N, class T>
372 | inline void assign(const Vec<N,T> &a, T &a0, T &a1, T &a2)
373 | { 
374 |    assert(N==3);
375 |    a0=a.v[0]; a1=a.v[1]; a2=a.v[2];
376 | }
377 | 
378 | template<unsigned int N, class T>
379 | inline void assign(const Vec<N,T> &a, T &a0, T &a1, T &a2, T &a3)
380 | { 
381 |    assert(N==4);
382 |    a0=a.v[0]; a1=a.v[1]; a2=a.v[2]; a3=a.v[3];
383 | }
384 | 
385 | template<unsigned int N, class T>
386 | inline void assign(const Vec<N,T> &a, T &a0, T &a1, T &a2, T &a3, T &a4, T &a5)
387 | { 
388 |    assert(N==6);
389 |    a0=a.v[0]; a1=a.v[1]; a2=a.v[2]; a3=a.v[3]; a4=a.v[4]; a5=a.v[5];
390 | }
391 | 
392 | template<unsigned int N, class T>
393 | inline Vec<N,int> round(const Vec<N,T> &a)
394 | { 
395 |    Vec<N,int> rounded;
396 |    for(unsigned int i=0; i<N; ++i)
397 |       rounded.v[i]=lround(a.v[i]);
398 |    return rounded; 
399 | }
400 | 
401 | template<unsigned int N, class T>
402 | inline Vec<N,int> floor(const Vec<N,T> &a)
403 | { 
404 |    Vec<N,int> rounded;
405 |    for(unsigned int i=0; i<N; ++i)
406 |       rounded.v[i]=(int)floor(a.v[i]);
407 |    return rounded; 
408 | }
409 | 
410 | template<unsigned int N, class T>
411 | inline Vec<N,int> ceil(const Vec<N,T> &a)
412 | { 
413 |    Vec<N,int> rounded;
414 |    for(unsigned int i=0; i<N; ++i)
415 |       rounded.v[i]=(int)ceil(a.v[i]);
416 |    return rounded; 
417 | }
418 | 
419 | template<unsigned int N, class T>
420 | inline Vec<N,T> fabs(const Vec<N,T> &a)
421 | { 
422 |    Vec<N,T> result;
423 |    for(unsigned int i=0; i<N; ++i)
424 |       result.v[i]=fabs(a.v[i]);
425 |    return result; 
426 | }
427 | 
428 | template<unsigned int N, class T>
429 | inline void minmax(const Vec<N,T> &x0, const Vec<N,T> &x1, Vec<N,T> &xmin, Vec<N,T> &xmax)
430 | {
431 |    for(unsigned int i=0; i<N; ++i)
432 |       minmax(x0.v[i], x1.v[i], xmin.v[i], xmax.v[i]);
433 | }
434 | 
435 | template<unsigned int N, class T>
436 | inline void minmax(const Vec<N,T> &x0, const Vec<N,T> &x1, const Vec<N,T> &x2, Vec<N,T> &xmin, Vec<N,T> &xmax)
437 | {
438 |    for(unsigned int i=0; i<N; ++i)
439 |       minmax(x0.v[i], x1.v[i], x2.v[i], xmin.v[i], xmax.v[i]);
440 | }
441 | 
442 | template<unsigned int N, class T>
443 | inline void minmax(const Vec<N,T> &x0, const Vec<N,T> &x1, const Vec<N,T> &x2, const Vec<N,T> &x3,
444 |                    Vec<N,T> &xmin, Vec<N,T> &xmax)
445 | {
446 |    for(unsigned int i=0; i<N; ++i)
447 |       minmax(x0.v[i], x1.v[i], x2.v[i], x3.v[i], xmin.v[i], xmax.v[i]);
448 | }
449 | 
450 | template<unsigned int N, class T>
451 | inline void minmax(const Vec<N,T> &x0, const Vec<N,T> &x1, const Vec<N,T> &x2, const Vec<N,T> &x3, const Vec<N,T> &x4,
452 |                    Vec<N,T> &xmin, Vec<N,T> &xmax)
453 | {
454 |    for(unsigned int i=0; i<N; ++i)
455 |       minmax(x0.v[i], x1.v[i], x2.v[i], x3.v[i], x4.v[i], xmin.v[i], xmax.v[i]);
456 | }
457 | 
458 | template<unsigned int N, class T>
459 | inline void minmax(const Vec<N,T> &x0, const Vec<N,T> &x1, const Vec<N,T> &x2, const Vec<N,T> &x3, const Vec<N,T> &x4,
460 |                    const Vec<N,T> &x5, Vec<N,T> &xmin, Vec<N,T> &xmax)
461 | {
462 |    for(unsigned int i=0; i<N; ++i)
463 |       minmax(x0.v[i], x1.v[i], x2.v[i], x3.v[i], x4.v[i], x5.v[i], xmin.v[i], xmax.v[i]);
464 | }
465 | 
466 | template<unsigned int N, class T>
467 | inline void update_minmax(const Vec<N,T> &x, Vec<N,T> &xmin, Vec<N,T> &xmax)
468 | {
469 |    for(unsigned int i=0; i<N; ++i) update_minmax(x[i], xmin[i], xmax[i]);
470 | }
471 | 
472 | #endif
473 | 


--------------------------------------------------------------------------------