| 52 | |
| 53 | |
| 54 | bool DistanceTable::set(uint8_t x, uint8_t y, float value ) |
| 55 | { |
| 56 | // comment next line to skip range check (squeeze performance) |
| 57 | if ( (x >= _dimension) || (y >= _dimension)) return false; |
| 58 | if (x == y) return (value == 0.0); |
| 59 | |
| 60 | if ( x < y ) |
| 61 | { |
| 62 | uint8_t t = x; x = y; y = t; // swap |
| 63 | if (_invert) value = -value; |
| 64 | } |
| 65 | // prevent overflow by moving to 16 bit |
| 66 | uint16_t index = x; |
| 67 | index = (index * (index - 1)) / 2 + y; |
| 68 | _distanceTable[index] = value; |
| 69 | return true; |
| 70 | }; |
| 71 | |
| 72 | |
| 73 | float DistanceTable::get (uint8_t x, uint8_t y) |