init

util/Vec2.hpp

@@ -0,0 +1,286 @@
+#ifndef VEC2_HPP
+#define VEC2_HPP
+
+#include <cmath>
+#include <algorithm>
+#include <string>
+
+class Vec2 {
+    public:
+    float x, y;
+    Vec2() : x(0), y(0) {}
+    Vec2(float x, float y) : x(x), y(y) {}
+    
+    Vec2 operator+(const Vec2& other) const {
+        return Vec2(x + other.x, y + other.y);
+    }
+    
+    Vec2 operator-(const Vec2& other) const {
+        return Vec2(x - other.x, y - other.y);
+    }
+    
+    Vec2 operator*(const Vec2& other) const {
+        return Vec2(x * other.x, y * other.y);
+    }
+    
+    Vec2 operator/(const Vec2& other) const {
+        return Vec2(x / other.x, y / other.y);
+    }
+
+    Vec2 operator+(float scalar) const {
+        return Vec2(x + scalar, y + scalar);
+    }
+
+    Vec2 operator-(float scalar) const {
+        return Vec2(x - scalar, y - scalar);
+    }
+    
+    Vec2 operator-() const {
+        return Vec2(-x, -y);
+    }
+
+    Vec2 operator*(float scalar) const {
+        return Vec2(x * scalar, y * scalar);
+    }
+
+    Vec2 operator/(float scalar) const {
+        return Vec2(x / scalar, y / scalar);
+    }
+
+    Vec2& operator=(float scalar) {
+        x = y = scalar;
+        return *this;
+    }
+    
+    Vec2& operator+=(const Vec2& other) {
+        x += other.x;
+        y += other.y;
+        return *this;
+    }
+    
+    Vec2& operator-=(const Vec2& other) {
+        x -= other.x;
+        y -= other.y;
+        return *this;
+    }
+    
+    Vec2& operator*=(const Vec2& other) {
+        x *= other.x;
+        y *= other.y;
+        return *this;
+    }
+    
+    Vec2& operator/=(const Vec2& other) {
+        x /= other.x;
+        y /= other.y;
+        return *this;
+    }
+    
+    Vec2& operator+=(float scalar) {
+        x += scalar;
+        y += scalar;
+        return *this;
+    }
+    
+    Vec2& operator-=(float scalar) {
+        x -= scalar;
+        y -= scalar;
+        return *this;
+    }
+    
+    Vec2& operator*=(float scalar) {
+        x *= scalar;
+        y *= scalar;
+        return *this;
+    }
+    
+    Vec2& operator/=(float scalar) {
+        x /= scalar;
+        y /= scalar;
+        return *this;
+    }
+
+    float dot(const Vec2& other) const {
+        return x * other.x + y * other.y;
+    }
+
+    float length() const {
+        return std::sqrt(x * x + y * y);
+    }
+    
+    float lengthSquared() const {
+        return x * x + y * y;
+    }
+    
+    float distance(const Vec2& other) const {
+        return (*this - other).length();
+    }
+    
+    float distanceSquared(const Vec2& other) const {
+        Vec2 diff = *this - other;
+        return diff.x * diff.x + diff.y * diff.y;
+    }
+
+    Vec2 normalized() const {
+        float len = length();
+        if (len > 0) {
+            return *this / len;
+        }
+        return *this;
+    }
+    
+    bool operator==(const Vec2& other) const {
+        return x == other.x && y == other.y;
+    }
+    
+    bool operator!=(const Vec2& other) const {
+        return x != other.x || y != other.y;
+    }
+    
+    bool operator<(const Vec2& other) const {
+        return (x < other.x) || (x == other.x && y < other.y);
+    }
+    
+    bool operator<=(const Vec2& other) const {
+        return (x < other.x) || (x == other.x && y <= other.y);
+    }
+    
+    bool operator>(const Vec2& other) const {
+        return (x > other.x) || (x == other.x && y > other.y);
+    }
+    
+    bool operator>=(const Vec2& other) const {
+        return (x > other.x) || (x == other.x && y >= other.y);
+    }
+    
+    Vec2 abs() const {
+        return Vec2(std::abs(x), std::abs(y));
+    }
+    
+    Vec2 floor() const {
+        return Vec2(std::floor(x), std::floor(y));
+    }
+    
+    Vec2 ceil() const {
+        return Vec2(std::ceil(x), std::ceil(y));
+    }
+    
+    Vec2 round() const {
+        return Vec2(std::round(x), std::round(y));
+    }
+    
+    Vec2 min(const Vec2& other) const {
+        return Vec2(std::min(x, other.x), std::min(y, other.y));
+    }
+    
+    Vec2 max(const Vec2& other) const {
+        return Vec2(std::max(x, other.x), std::max(y, other.y));
+    }
+    
+    Vec2 clamp(const Vec2& minVal, const Vec2& maxVal) const {
+        return Vec2(
+            std::clamp(x, minVal.x, maxVal.x),
+            std::clamp(y, minVal.y, maxVal.y)
+        );
+    }
+    
+    Vec2 clamp(float minVal, float maxVal) const {
+        return Vec2(
+            std::clamp(x, minVal, maxVal),
+            std::clamp(y, minVal, maxVal)
+        );
+    }
+    
+    bool isZero(float epsilon = 1e-10f) const {
+        return std::abs(x) < epsilon && std::abs(y) < epsilon;
+    }
+    
+    bool equals(const Vec2& other, float epsilon = 1e-10f) const {
+        return std::abs(x - other.x) < epsilon && 
+               std::abs(y - other.y) < epsilon;
+    }
+    
+    friend Vec2 operator+(float scalar, const Vec2& vec) {
+        return Vec2(scalar + vec.x, scalar + vec.y);
+    }
+    
+    friend Vec2 operator-(float scalar, const Vec2& vec) {
+        return Vec2(scalar - vec.x, scalar - vec.y);
+    }
+    
+    friend Vec2 operator*(float scalar, const Vec2& vec) {
+        return Vec2(scalar * vec.x, scalar * vec.y);
+    }
+    
+    friend Vec2 operator/(float scalar, const Vec2& vec) {
+        return Vec2(scalar / vec.x, scalar / vec.y);
+    }
+
+    Vec2 perpendicular() const {
+        return Vec2(-y, x);
+    }
+    
+    Vec2 reflect(const Vec2& normal) const {
+        return *this - 2.0f * this->dot(normal) * normal;
+    }
+    
+    Vec2 lerp(const Vec2& other, float t) const {
+        t = std::clamp(t, 0.0f, 1.0f);
+        return *this + (other - *this) * t;
+    }
+    
+    Vec2 slerp(const Vec2& other, float t) const {
+        t = std::clamp(t, 0.0f, 1.0f);
+        float dot = this->dot(other);
+        dot = std::clamp(dot, -1.0f, 1.0f);
+        
+        float theta = std::acos(dot) * t;
+        Vec2 relative = other - *this * dot;
+        relative = relative.normalized();
+        
+        return (*this * std::cos(theta)) + (relative * std::sin(theta));
+    }
+    
+    Vec2 rotate(float angle) const {
+        float cosA = std::cos(angle);
+        float sinA = std::sin(angle);
+        return Vec2(x * cosA - y * sinA, x * sinA + y * cosA);
+    }
+    
+    float angle() const {
+        return std::atan2(y, x);
+    }
+    
+    float angleTo(const Vec2& other) const {
+        return std::acos(this->dot(other) / (this->length() * other.length()));
+    }
+    
+    float& operator[](int index) {
+        return (&x)[index];
+    }
+    
+    const float& operator[](int index) const {
+        return (&x)[index];
+    }
+    
+    std::string toString() const {
+        return "(" + std::to_string(x) + ", " + std::to_string(y) + ")";
+    }
+    
+};
+
+inline std::ostream& operator<<(std::ostream& os, const Vec2& vec) {
+    os << vec.toString();
+    return os;
+}
+
+namespace std {
+    template<>
+    struct hash<Vec2> {
+        size_t operator()(const Vec2& v) const {
+            return hash<float>()(v.x) ^ (hash<float>()(v.y) << 1);
+        }
+    };
+}
+
+#endif

util/bmpwriter.hpp

@@ -0,0 +1,167 @@
+#ifndef BMP_WRITER_HPP
+#define BMP_WRITER_HPP
+
+#include <vector>
+#include <fstream>
+#include <cstring>
+#include <string>
+#include <algorithm>
+#include "vec3.hpp"
+
+class BMPWriter {
+private:
+    #pragma pack(push, 1)
+    struct BMPHeader {
+        uint16_t signature = 0x4D42; // "BM"
+        uint32_t fileSize;
+        uint16_t reserved1 = 0;
+        uint16_t reserved2 = 0;
+        uint32_t dataOffset = 54;
+    };
+    
+    struct BMPInfoHeader {
+        uint32_t headerSize = 40;
+        int32_t width;
+        int32_t height;
+        uint16_t planes = 1;
+        uint16_t bitsPerPixel = 24;
+        uint32_t compression = 0;
+        uint32_t imageSize;
+        int32_t xPixelsPerMeter = 0;
+        int32_t yPixelsPerMeter = 0;
+        uint32_t colorsUsed = 0;
+        uint32_t importantColors = 0;
+    };
+    #pragma pack(pop)
+
+public:
+    // Save a 2D vector of Vec3 (RGB) colors as BMP
+    // Vec3 components: x = red, y = green, z = blue (values in range [0,1])
+    static bool saveBMP(const std::string& filename, const std::vector<std::vector<Vec3>>& pixels) {
+        if (pixels.empty() || pixels[0].empty()) {
+            return false;
+        }
+        
+        int height = static_cast<int>(pixels.size());
+        int width = static_cast<int>(pixels[0].size());
+        
+        // Validate that all rows have the same width
+        for (const auto& row : pixels) {
+            if (row.size() != width) {
+                return false;
+            }
+        }
+        
+        return saveBMP(filename, pixels, width, height);
+    }
+    
+    // Alternative interface with width/height and flat vector (row-major order)
+    static bool saveBMP(const std::string& filename, const std::vector<Vec3>& pixels, int width, int height) {
+        if (pixels.size() != width * height) {
+            return false;
+        }
+        
+        // Convert to 2D vector format
+        std::vector<std::vector<Vec3>> pixels2D(height, std::vector<Vec3>(width));
+        for (int y = 0; y < height; ++y) {
+            for (int x = 0; x < width; ++x) {
+                pixels2D[y][x] = pixels[y * width + x];
+            }
+        }
+        
+        return saveBMP(filename, pixels2D, width, height);
+    }
+    
+    // Save from 1D vector of uint8_t pixels (BGR order: pixels[i]=b, pixels[i+1]=g, pixels[i+2]=r)
+    static bool saveBMP(const std::string& filename, const std::vector<uint8_t>& pixels, int width, int height) {
+        if (pixels.size() != width * height * 3) {
+            return false;
+        }
+        
+        BMPHeader header;
+        BMPInfoHeader infoHeader;
+        
+        int rowSize = (width * 3 + 3) & ~3; // 24-bit, padded to 4 bytes
+        int imageSize = rowSize * height;
+        
+        header.fileSize = sizeof(BMPHeader) + sizeof(BMPInfoHeader) + imageSize;
+        infoHeader.width = width;
+        infoHeader.height = height;
+        infoHeader.imageSize = imageSize;
+        
+        std::ofstream file(filename, std::ios::binary);
+        if (!file) {
+            return false;
+        }
+        
+        file.write(reinterpret_cast<const char*>(&header), sizeof(header));
+        file.write(reinterpret_cast<const char*>(&infoHeader), sizeof(infoHeader));
+        
+        // Write pixel data (BMP stores pixels bottom-to-top)
+        std::vector<uint8_t> row(rowSize, 0);
+        for (int y = height - 1; y >= 0; --y) {
+            const uint8_t* srcRow = pixels.data() + (y * width * 3);
+            
+            // Copy and rearrange if necessary (input is already in BGR order)
+            for (int x = 0; x < width; ++x) {
+                int srcOffset = x * 3;
+                int dstOffset = x * 3;
+                
+                // Input is already BGR: pixels[i]=b, pixels[i+1]=g, pixels[i+2]=r
+                // So we can copy directly
+                row[dstOffset] = srcRow[srcOffset];     // B
+                row[dstOffset + 1] = srcRow[srcOffset + 1]; // G
+                row[dstOffset + 2] = srcRow[srcOffset + 2]; // R
+            }
+            file.write(reinterpret_cast<const char*>(row.data()), rowSize);
+        }
+        
+        return true;
+    }
+    
+private:
+    static bool saveBMP(const std::string& filename, const std::vector<std::vector<Vec3>>& pixels, int width, int height) {
+        BMPHeader header;
+        BMPInfoHeader infoHeader;
+        
+        int rowSize = (width * 3 + 3) & ~3; // 24-bit, padded to 4 bytes
+        int imageSize = rowSize * height;
+        
+        header.fileSize = sizeof(BMPHeader) + sizeof(BMPInfoHeader) + imageSize;
+        infoHeader.width = width;
+        infoHeader.height = height;
+        infoHeader.imageSize = imageSize;
+        
+        std::ofstream file(filename, std::ios::binary);
+        if (!file) {
+            return false;
+        }
+        
+        file.write(reinterpret_cast<const char*>(&header), sizeof(header));
+        file.write(reinterpret_cast<const char*>(&infoHeader), sizeof(infoHeader));
+        
+        // Write pixel data (BMP stores pixels bottom-to-top)
+        std::vector<uint8_t> row(rowSize, 0);
+        for (int y = height - 1; y >= 0; --y) {
+            for (int x = 0; x < width; ++x) {
+                const Vec3& color = pixels[y][x];
+                
+                // Convert from [0,1] float to [0,255] uint8_t
+                uint8_t r = static_cast<uint8_t>(std::clamp(color.x * 255.0f, 0.0f, 255.0f));
+                uint8_t g = static_cast<uint8_t>(std::clamp(color.y * 255.0f, 0.0f, 255.0f));
+                uint8_t b = static_cast<uint8_t>(std::clamp(color.z * 255.0f, 0.0f, 255.0f));
+                
+                // BMP is BGR order
+                int pixelOffset = x * 3;
+                row[pixelOffset] = b;
+                row[pixelOffset + 1] = g;
+                row[pixelOffset + 2] = r;
+            }
+            file.write(reinterpret_cast<const char*>(row.data()), rowSize);
+        }
+        
+        return true;
+    }
+};
+
+#endif

util/grid2.hpp

@@ -0,0 +1,231 @@
+#ifndef GRID2_HPP
+#define GRID2_HPP
+
+#include "Vec2.hpp"
+#include "Vec4.hpp"
+#include <vector>
+#include <cstdint>
+#include <algorithm>
+#include <stdexcept>
+
+class Grid2 {
+public:
+    std::vector<Vec2> positions;
+    std::vector<Vec4> colors;
+    
+    Grid2() = default;
+    
+    // Constructor with initial size
+    Grid2(size_t size) {
+        positions.resize(size);
+        colors.resize(size);
+    }
+    
+    // Add a point with position and color
+    void addPoint(const Vec2& position, const Vec4& color) {
+        positions.push_back(position);
+        colors.push_back(color);
+    }
+    
+    // Clear all points
+    void clear() {
+        positions.clear();
+        colors.clear();
+    }
+    
+    // Get number of points
+    size_t size() const {
+        return positions.size();
+    }
+    
+    // Check if grid is empty
+    bool empty() const {
+        return positions.empty();
+    }
+    
+    // Resize the grid
+    void resize(size_t newSize) {
+        positions.resize(newSize);
+        colors.resize(newSize);
+    }
+    
+    // Render to RGB image data
+    std::vector<uint8_t> renderToRGB(int width, int height, const Vec4& backgroundColor = Vec4(0, 0, 0, 1)) const {
+        if (width <= 0 || height <= 0) {
+            throw std::invalid_argument("Width and height must be positive");
+        }
+        
+        std::vector<uint8_t> imageData(width * height * 3);
+        
+        // Initialize with background color
+        uint8_t bgR, bgG, bgB;
+        backgroundColor.toUint8(bgR, bgG, bgB);
+        
+        for (int i = 0; i < width * height * 3; i += 3) {
+            imageData[i] = bgR;
+            imageData[i + 1] = bgG;
+            imageData[i + 2] = bgB;
+        }
+        
+        // Find the bounding box of all points to map to pixel coordinates
+        if (positions.empty()) {
+            return imageData;
+        }
+        
+        Vec2 minPos = positions[0];
+        Vec2 maxPos = positions[0];
+        
+        for (const auto& pos : positions) {
+            minPos = minPos.min(pos);
+            maxPos = maxPos.max(pos);
+        }
+        
+        // Add a small margin to avoid division by zero and edge issues
+        Vec2 size = maxPos - minPos;
+        if (size.x < 1e-10f) size.x = 1.0f;
+        if (size.y < 1e-10f) size.y = 1.0f;
+        
+        float margin = 0.05f; // 5% margin
+        minPos -= size * margin;
+        maxPos += size * margin;
+        size = maxPos - minPos;
+        
+        // Render each point
+        for (size_t i = 0; i < positions.size(); i++) {
+            const Vec2& pos = positions[i];
+            const Vec4& color = colors[i];
+            
+            // Convert world coordinates to pixel coordinates
+            float normalizedX = (pos.x - minPos.x) / size.x;
+            float normalizedY = 1.0f - (pos.y - minPos.y) / size.y; // Flip Y for image coordinates
+            
+            int pixelX = static_cast<int>(normalizedX * width);
+            int pixelY = static_cast<int>(normalizedY * height);
+            
+            // Clamp to image bounds
+            pixelX = std::clamp(pixelX, 0, width - 1);
+            pixelY = std::clamp(pixelY, 0, height - 1);
+            
+            // Convert color to RGB
+            uint8_t r, g, b;
+            color.toUint8(r, g, b);
+            
+            // Set pixel color
+            int index = (pixelY * width + pixelX) * 3;
+            imageData[index] = r;
+            imageData[index + 1] = g;
+            imageData[index + 2] = b;
+        }
+        
+        return imageData;
+    }
+    
+    // Render to RGBA image data (with alpha channel)
+    std::vector<uint8_t> renderToRGBA(int width, int height, const Vec4& backgroundColor = Vec4(0, 0, 0, 1)) const {
+        if (width <= 0 || height <= 0) {
+            throw std::invalid_argument("Width and height must be positive");
+        }
+        
+        std::vector<uint8_t> imageData(width * height * 4);
+        
+        // Initialize with background color
+        uint8_t bgR, bgG, bgB, bgA;
+        backgroundColor.toUint8(bgR, bgG, bgB, bgA);
+        
+        for (int i = 0; i < width * height * 4; i += 4) {
+            imageData[i] = bgR;
+            imageData[i + 1] = bgG;
+            imageData[i + 2] = bgB;
+            imageData[i + 3] = bgA;
+        }
+        
+        if (positions.empty()) {
+            return imageData;
+        }
+        
+        // Find the bounding box (same as RGB version)
+        Vec2 minPos = positions[0];
+        Vec2 maxPos = positions[0];
+        
+        for (const auto& pos : positions) {
+            minPos = minPos.min(pos);
+            maxPos = maxPos.max(pos);
+        }
+        
+        Vec2 size = maxPos - minPos;
+        if (size.x < 1e-10f) size.x = 1.0f;
+        if (size.y < 1e-10f) size.y = 1.0f;
+        
+        float margin = 0.05f;
+        minPos -= size * margin;
+        maxPos += size * margin;
+        size = maxPos - minPos;
+        
+        // Render each point
+        for (size_t i = 0; i < positions.size(); i++) {
+            const Vec2& pos = positions[i];
+            const Vec4& color = colors[i];
+            
+            float normalizedX = (pos.x - minPos.x) / size.x;
+            float normalizedY = 1.0f - (pos.y - minPos.y) / size.y;
+            
+            int pixelX = static_cast<int>(normalizedX * width);
+            int pixelY = static_cast<int>(normalizedY * height);
+            
+            pixelX = std::clamp(pixelX, 0, width - 1);
+            pixelY = std::clamp(pixelY, 0, height - 1);
+            
+            uint8_t r, g, b, a;
+            color.toUint8(r, g, b, a);
+            
+            int index = (pixelY * width + pixelX) * 4;
+            imageData[index] = r;
+            imageData[index + 1] = g;
+            imageData[index + 2] = b;
+            imageData[index + 3] = a;
+        }
+        
+        return imageData;
+    }
+    
+    // Get the bounding box of all positions
+    void getBoundingBox(Vec2& minPos, Vec2& maxPos) const {
+        if (positions.empty()) {
+            minPos = Vec2(0, 0);
+            maxPos = Vec2(0, 0);
+            return;
+        }
+        
+        minPos = positions[0];
+        maxPos = positions[0];
+        
+        for (const auto& pos : positions) {
+            minPos = minPos.min(pos);
+            maxPos = maxPos.max(pos);
+        }
+    }
+    
+    // Scale all positions to fit within a specified range
+    void normalizePositions(const Vec2& targetMin = Vec2(-1, -1), const Vec2& targetMax = Vec2(1, 1)) {
+        if (positions.empty()) return;
+        
+        Vec2 currentMin, currentMax;
+        getBoundingBox(currentMin, currentMax);
+        
+        Vec2 currentSize = currentMax - currentMin;
+        Vec2 targetSize = targetMax - targetMin;
+        
+        if (currentSize.x < 1e-10f) currentSize.x = 1.0f;
+        if (currentSize.y < 1e-10f) currentSize.y = 1.0f;
+        
+        for (auto& pos : positions) {
+            float normalizedX = (pos.x - currentMin.x) / currentSize.x;
+            float normalizedY = (pos.y - currentMin.y) / currentSize.y;
+            
+            pos.x = targetMin.x + normalizedX * targetSize.x;
+            pos.y = targetMin.y + normalizedY * targetSize.y;
+        }
+    }
+};
+
+#endif

util/ray.cpp

@@ -0,0 +1,27 @@
+#ifndef RAY_HPP
+#define RAY_HPP
+
+#include "Vec2.hpp"
+#include "Vec3.hpp"
+#include "Vec4.hpp"
+#include "ray2.hpp"
+#include "ray3.hpp"
+#include "ray4.hpp"
+
+// Stream operators for rays
+inline std::ostream& operator<<(std::ostream& os, const Ray2& ray) {
+    os << ray.toString();
+    return os;
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Ray3& ray) {
+    os << ray.toString();
+    return os;
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Ray4& ray) {
+    os << ray.toString();
+    return os;
+}
+
+#endif

util/ray2.hpp

@@ -0,0 +1,59 @@
+#ifndef RAY2_HPP
+#define RAY2_HPP
+
+#include "Vec2.hpp"
+
+class Ray2 {
+public:
+    Vec2 origin;
+    Vec2 direction;
+    
+    Ray2() : origin(Vec2()), direction(Vec2(1, 0)) {}
+    Ray2(const Vec2& origin, const Vec2& direction) 
+        : origin(origin), direction(direction.normalized()) {}
+    
+    // Get point at parameter t along the ray
+    Vec2 at(float t) const {
+        return origin + direction * t;
+    }
+    
+    // Reflect ray off a surface with given normal
+    Ray2 reflect(const Vec2& point, const Vec2& normal) const {
+        Vec2 reflectedDir = direction.reflect(normal);
+        return Ray2(point, reflectedDir);
+    }
+    
+    // Check if ray intersects with a circle
+    bool intersectsCircle(const Vec2& center, float radius, float& t1, float& t2) const {
+        Vec2 oc = origin - center;
+        float a = direction.dot(direction);
+        float b = 2.0f * oc.dot(direction);
+        float c = oc.dot(oc) - radius * radius;
+        
+        float discriminant = b * b - 4 * a * c;
+        
+        if (discriminant < 0) {
+            return false;
+        }
+        
+        discriminant = std::sqrt(discriminant);
+        t1 = (-b - discriminant) / (2.0f * a);
+        t2 = (-b + discriminant) / (2.0f * a);
+        
+        return true;
+    }
+    
+    // Get the distance from a point to this ray
+    float distanceToPoint(const Vec2& point) const {
+        Vec2 pointToOrigin = point - origin;
+        float projection = pointToOrigin.dot(direction);
+        Vec2 closestPoint = origin + direction * projection;
+        return point.distance(closestPoint);
+    }
+    
+    std::string toString() const {
+        return "Ray2(origin: " + origin.toString() + ", direction: " + direction.toString() + ")";
+    }
+};
+
+#endif

util/ray3.hpp

@@ -0,0 +1,73 @@
+#ifndef RAY3_HPP
+#define RAY3_HPP
+
+#include "Vec3.hpp"
+
+class Ray3 {
+public:
+    Vec3 origin;
+    Vec3 direction;
+    
+    Ray3() : origin(Vec3()), direction(Vec3(1, 0, 0)) {}
+    Ray3(const Vec3& origin, const Vec3& direction) 
+        : origin(origin), direction(direction.normalized()) {}
+    
+    // Get point at parameter t along the ray
+    Vec3 at(float t) const {
+        return origin + direction * t;
+    }
+    
+    // Reflect ray off a surface with given normal
+    Ray3 reflect(const Vec3& point, const Vec3& normal) const {
+        Vec3 reflectedDir = direction.reflect(normal);
+        return Ray3(point, reflectedDir);
+    }
+    
+    // Check if ray intersects with a sphere
+    bool intersectsSphere(const Vec3& center, float radius, float& t1, float& t2) const {
+        Vec3 oc = origin - center;
+        float a = direction.dot(direction);
+        float b = 2.0f * oc.dot(direction);
+        float c = oc.dot(oc) - radius * radius;
+        
+        float discriminant = b * b - 4 * a * c;
+        
+        if (discriminant < 0) {
+            return false;
+        }
+        
+        discriminant = std::sqrt(discriminant);
+        t1 = (-b - discriminant) / (2.0f * a);
+        t2 = (-b + discriminant) / (2.0f * a);
+        
+        return true;
+    }
+    
+    // Check if ray intersects with a plane (defined by point and normal)
+    bool intersectsPlane(const Vec3& planePoint, const Vec3& planeNormal, float& t) const {
+        float denom = planeNormal.dot(direction);
+        
+        if (std::abs(denom) < 1e-10f) {
+            return false; // Ray is parallel to plane
+        }
+        
+        t = planeNormal.dot(planePoint - origin) / denom;
+        return t >= 0;
+    }
+    
+    // Get the distance from a point to this ray
+    float distanceToPoint(const Vec3& point) const {
+        Vec3 pointToOrigin = point - origin;
+        Vec3 crossProduct = direction.cross(pointToOrigin);
+        return crossProduct.length() / direction.length();
+    }
+    
+    // Transform ray by a 4x4 matrix (for perspective/affine transformations)
+    Ray3 transform(const class Mat4& matrix) const;
+    
+    std::string toString() const {
+        return "Ray3(origin: " + origin.toString() + ", direction: " + direction.toString() + ")";
+    }
+};
+
+#endif

util/ray4.hpp

@@ -0,0 +1,52 @@
+#ifndef RAY4_HPP
+#define RAY4_HPP
+
+#include "vec4.hpp"
+
+class Ray4 {
+public:
+    Vec4 origin;
+    Vec4 direction;
+    
+    Ray4() : origin(Vec4()), direction(Vec4(1, 0, 0, 0)) {}
+    Ray4(const Vec4& origin, const Vec4& direction) 
+        : origin(origin), direction(direction.normalized()) {}
+    
+    // Get point at parameter t along the ray (in 4D space)
+    Vec4 at(float t) const {
+        return origin + direction * t;
+    }
+    
+    // Get 3D projection of the ray (homogeneous coordinates)
+    // Ray3 projectTo3D() const {
+    //     Vec3 projOrigin = origin.homogenized().xyz();
+    //     Vec3 projDirection = direction.homogenized().xyz().normalized();
+    //     return Ray3(projOrigin, projDirection);
+    // }
+    
+    // Get the distance from a point to this ray in 4D space
+    float distanceToPoint(const Vec4& point) const {
+        Vec4 pointToOrigin = point - origin;
+        float projection = pointToOrigin.dot(direction);
+        Vec4 closestPoint = origin + direction * projection;
+        return point.distance(closestPoint);
+    }
+    
+    // Check if this 4D ray intersects with a 3D hyperplane
+    bool intersectsHyperplane(const Vec4& planePoint, const Vec4& planeNormal, float& t) const {
+        float denom = planeNormal.dot(direction);
+        
+        if (std::abs(denom) < 1e-10f) {
+            return false; // Ray is parallel to hyperplane
+        }
+        
+        t = planeNormal.dot(planePoint - origin) / denom;
+        return true;
+    }
+    
+    std::string toString() const {
+        return "Ray4(origin: " + origin.toString() + ", direction: " + direction.toString() + ")";
+    }
+};
+
+#endif

util/timing_decorator.cpp

@@ -0,0 +1,115 @@
+#include "timing_decorator.hpp"
+#include <cmath>
+
+std::unordered_map<std::string, FunctionTimer::TimingStats> FunctionTimer::stats_;
+
+void FunctionTimer::recordTiming(const std::string& func_name, double elapsed_seconds) {
+    auto& stat = stats_[func_name];
+    stat.call_count++;
+    stat.total_time += elapsed_seconds;
+    stat.timings.push_back(elapsed_seconds);
+}
+
+FunctionTimer::PercentileStats FunctionTimer::calculatePercentiles(const std::vector<double>& timings) {
+    PercentileStats result;
+    if (timings.empty()) return result;
+    
+    std::vector<double> sorted_timings = timings;
+    std::sort(sorted_timings.begin(), sorted_timings.end());
+    
+    auto percentile_index = [&](double p) -> size_t {
+        return std::min(sorted_timings.size() - 1, 
+                       static_cast<size_t>(p * sorted_timings.size() / 100.0));
+    };
+    
+    result.min = sorted_timings.front();
+    result.max = sorted_timings.back();
+    result.median = sorted_timings[percentile_index(50.0)];
+    result.p90 = sorted_timings[percentile_index(90.0)];
+    result.p95 = sorted_timings[percentile_index(95.0)];
+    result.p99 = sorted_timings[percentile_index(99.0)];
+    result.p99_9 = sorted_timings[percentile_index(99.9)];
+    
+    return result;
+}
+
+std::unordered_map<std::string, FunctionTimer::TimingStats> FunctionTimer::getStats(Mode mode) {
+    return stats_; // In C++ we return all data, filtering happens in print
+}
+
+void FunctionTimer::printStats(Mode mode) {
+    if (stats_.empty()) {
+        std::cout << "No timing statistics available." << std::endl;
+        return;
+    }
+
+    // Determine column widths
+    size_t func_col_width = 0;
+    for (const auto& [name, _] : stats_) {
+        func_col_width = std::max(func_col_width, name.length());
+    }
+    func_col_width = std::max(func_col_width, size_t(8)); // "Function"
+    
+    const int num_width = 12;
+
+    if (mode == Mode::BASIC) {
+        std::cout << "\nBasic Function Timing Statistics:" << std::endl;
+        std::cout << std::string(func_col_width + 3 * num_width + 8, '-') << std::endl;
+        
+        std::cout << std::left << std::setw(func_col_width) << "Function"
+                  << std::setw(num_width) << "Calls"
+                  << std::setw(num_width) << "Total (s)"
+                  << std::setw(num_width) << "Avg (s)" << std::endl;
+                  
+        std::cout << std::string(func_col_width + 3 * num_width + 8, '-') << std::endl;
+        
+        for (const auto& [func_name, data] : stats_) {
+            if (data.call_count > 0) {
+                std::cout << std::left << std::setw(func_col_width) << func_name
+                          << std::setw(num_width) << data.call_count
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << data.total_time
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << data.avg_time() << std::endl;
+            }
+        }
+        
+        std::cout << std::string(func_col_width + 3 * num_width + 8, '-') << std::endl;
+    } else { // ENHANCED mode
+        std::cout << "\nEnhanced Function Timing Statistics:" << std::endl;
+        size_t total_width = func_col_width + 8 * num_width + 8;
+        std::cout << std::string(total_width, '-') << std::endl;
+        
+        std::cout << std::left << std::setw(func_col_width) << "Function"
+                  << std::setw(num_width) << "Calls"
+                  << std::setw(num_width) << "Total (s)"
+                  << std::setw(num_width) << "Avg (s)"
+                  << std::setw(num_width) << "Min (s)"
+                  << std::setw(num_width) << "Median (s)"
+                  << std::setw(num_width) << "P99 (s)"
+                  << std::setw(num_width) << "P99.9 (s)"
+                  << std::setw(num_width) << "Max (s)" << std::endl;
+                  
+        std::cout << std::string(total_width, '-') << std::endl;
+        
+        for (const auto& [func_name, data] : stats_) {
+            if (data.call_count > 0) {
+                auto percentiles = calculatePercentiles(data.timings);
+                
+                std::cout << std::left << std::setw(func_col_width) << func_name
+                          << std::setw(num_width) << data.call_count
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << data.total_time
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << data.avg_time()
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << percentiles.min
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << percentiles.median
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << percentiles.p99
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << percentiles.p99_9
+                          << std::setw(num_width) << std::fixed << std::setprecision(6) << percentiles.max << std::endl;
+            }
+        }
+        
+        std::cout << std::string(total_width, '-') << std::endl;
+    }
+}
+
+void FunctionTimer::clearStats() {
+    stats_.clear();
+}

util/timing_decorator.hpp

@@ -0,0 +1,100 @@
+#pragma once
+
+#include <chrono>
+#include <unordered_map>
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <iostream>
+#include <iomanip>
+#include <sstream>
+#include <memory>
+#include <functional>
+
+class FunctionTimer {
+public:
+    enum class Mode { BASIC, ENHANCED };
+    
+    struct TimingStats {
+        size_t call_count = 0;
+        double total_time = 0.0;
+        std::vector<double> timings;
+        
+        double avg_time() const {
+            return call_count > 0 ? total_time / call_count : 0.0;
+        }
+    };
+    
+    struct PercentileStats {
+        double p99_9 = 0.0;
+        double p99 = 0.0;
+        double p95 = 0.0;
+        double p90 = 0.0;
+        double max = 0.0;
+        double min = 0.0;
+        double median = 0.0;
+    };
+    
+    // Record timing for a function
+    static void recordTiming(const std::string& func_name, double elapsed_seconds);
+    
+    // Get statistics
+    static std::unordered_map<std::string, TimingStats> getStats(Mode mode = Mode::BASIC);
+    
+    // Print statistics
+    static void printStats(Mode mode = Mode::ENHANCED);
+    
+    // Clear all statistics
+    static void clearStats();
+
+private:
+    static std::unordered_map<std::string, TimingStats> stats_;
+    
+    static PercentileStats calculatePercentiles(const std::vector<double>& timings);
+};
+
+// Macro to easily time functions - similar to Python decorator
+#define TIME_FUNCTION auto function_timer_scoped_ = ScopedFunctionTimer(__func__)
+
+// Scoped timer for RAII-style timing
+class ScopedFunctionTimer {
+public:
+    ScopedFunctionTimer(const std::string& func_name) 
+        : func_name_(func_name), start_(std::chrono::steady_clock::now()) {}
+    
+    ~ScopedFunctionTimer() {
+        auto end = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start_);
+        FunctionTimer::recordTiming(func_name_, elapsed.count() / 1000000.0);
+    }
+
+private:
+    std::string func_name_;
+    std::chrono::steady_clock::time_point start_;
+};
+
+// Template decorator for functions (similar to Python)
+template<typename Func, typename... Args>
+auto time_function_decorator(const std::string& func_name, Func&& func, Args&&... args) {
+    auto start = std::chrono::steady_clock::now();
+    
+    if constexpr (std::is_void_v<std::invoke_result_t<Func, Args...>>) {
+        // Void return type
+        std::invoke(std::forward<Func>(func), std::forward<Args>(args)...);
+        auto end = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        FunctionTimer::recordTiming(func_name, elapsed.count() / 1000000.0);
+    } else {
+        // Non-void return type
+        auto result = std::invoke(std::forward<Func>(func), std::forward<Args>(args)...);
+        auto end = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        FunctionTimer::recordTiming(func_name, elapsed.count() / 1000000.0);
+        return result;
+    }
+}
+
+// Macro to create decorated functions
+#define DECORATE_FUNCTION(func) [&](auto&&... args) { \
+    return time_function_decorator(#func, func, std::forward<decltype(args)>(args)...); \
+}

util/vec.cpp

@@ -0,0 +1,19 @@
+#ifndef vec_hpp
+#define vec_hpp
+
+#include "Vec4.hpp"
+#include "Vec3.hpp"
+#include "Vec2.hpp"
+
+Vec4::Vec4(const Vec3& vec3, float w) : x(vec3.x), y(vec3.y), z(vec3.z), w(w) {}
+Vec3::Vec3(const Vec2& vec2, float z) : x(vec2.x), y(vec2.y), z(z) {}
+
+Vec3 Vec4::xyz() const {
+    return Vec3(x, y, z);
+}
+
+Vec3 Vec4::rgb() const {
+    return Vec3(r, g, b);
+}
+
+#endif

util/vec3.hpp

@@ -0,0 +1,322 @@
+#ifndef VEC3_HPP
+#define VEC3_HPP
+
+#include <cmath>
+#include <algorithm>
+#include <string>
+#include <ostream>
+
+class Vec3 {
+public:
+    float x, y, z;
+    
+    Vec3() : x(0), y(0), z(0) {}
+    Vec3(float x, float y, float z) : x(x), y(y), z(z) {}
+    Vec3(float scalar) : x(scalar), y(scalar), z(scalar) {}
+
+    Vec3(const class Vec2& vec2, float z = 0.0f);
+    
+    // Arithmetic operations
+    Vec3 operator+(const Vec3& other) const {
+        return Vec3(x + other.x, y + other.y, z + other.z);
+    }
+    
+    Vec3 operator-(const Vec3& other) const {
+        return Vec3(x - other.x, y - other.y, z - other.z);
+    }
+    
+    Vec3 operator*(const Vec3& other) const {
+        return Vec3(x * other.x, y * other.y, z * other.z);
+    }
+    
+    Vec3 operator/(const Vec3& other) const {
+        return Vec3(x / other.x, y / other.y, z / other.z);
+    }
+
+    Vec3 operator+(float scalar) const {
+        return Vec3(x + scalar, y + scalar, z + scalar);
+    }
+
+    Vec3 operator-(float scalar) const {
+        return Vec3(x - scalar, y - scalar, z - scalar);
+    }
+    
+    Vec3 operator-() const {
+        return Vec3(-x, -y, -z);
+    }
+
+    Vec3 operator*(float scalar) const {
+        return Vec3(x * scalar, y * scalar, z * scalar);
+    }
+
+    Vec3 operator/(float scalar) const {
+        return Vec3(x / scalar, y / scalar, z / scalar);
+    }
+
+    Vec3& operator=(float scalar) {
+        x = y = z = scalar;
+        return *this;
+    }
+    
+    Vec3& operator+=(const Vec3& other) {
+        x += other.x;
+        y += other.y;
+        z += other.z;
+        return *this;
+    }
+    
+    Vec3& operator-=(const Vec3& other) {
+        x -= other.x;
+        y -= other.y;
+        z -= other.z;
+        return *this;
+    }
+    
+    Vec3& operator*=(const Vec3& other) {
+        x *= other.x;
+        y *= other.y;
+        z *= other.z;
+        return *this;
+    }
+    
+    Vec3& operator/=(const Vec3& other) {
+        x /= other.x;
+        y /= other.y;
+        z /= other.z;
+        return *this;
+    }
+    
+    Vec3& operator+=(float scalar) {
+        x += scalar;
+        y += scalar;
+        z += scalar;
+        return *this;
+    }
+    
+    Vec3& operator-=(float scalar) {
+        x -= scalar;
+        y -= scalar;
+        z -= scalar;
+        return *this;
+    }
+    
+    Vec3& operator*=(float scalar) {
+        x *= scalar;
+        y *= scalar;
+        z *= scalar;
+        return *this;
+    }
+    
+    Vec3& operator/=(float scalar) {
+        x /= scalar;
+        y /= scalar;
+        z /= scalar;
+        return *this;
+    }
+
+    float dot(const Vec3& other) const {
+        return x * other.x + y * other.y + z * other.z;
+    }
+    
+    Vec3 cross(const Vec3& other) const {
+        return Vec3(
+            y * other.z - z * other.y,
+            z * other.x - x * other.z,
+            x * other.y - y * other.x
+        );
+    }
+
+    float length() const {
+        return std::sqrt(x * x + y * y + z * z);
+    }
+    
+    float lengthSquared() const {
+        return x * x + y * y + z * z;
+    }
+    
+    float distance(const Vec3& other) const {
+        return (*this - other).length();
+    }
+    
+    float distanceSquared(const Vec3& other) const {
+        Vec3 diff = *this - other;
+        return diff.x * diff.x + diff.y * diff.y + diff.z * diff.z;
+    }
+
+    Vec3 normalized() const {
+        float len = length();
+        if (len > 0) {
+            return *this / len;
+        }
+        return *this;
+    }
+    
+    bool operator==(const Vec3& other) const {
+        return x == other.x && y == other.y && z == other.z;
+    }
+    
+    bool operator!=(const Vec3& other) const {
+        return x != other.x || y != other.y || z != other.z;
+    }
+    
+    bool operator<(const Vec3& other) const {
+        return (x < other.x) || 
+               (x == other.x && y < other.y) || 
+               (x == other.x && y == other.y && z < other.z);
+    }
+    
+    bool operator<=(const Vec3& other) const {
+        return (x < other.x) || 
+               (x == other.x && y < other.y) || 
+               (x == other.x && y == other.y && z <= other.z);
+    }
+    
+    bool operator>(const Vec3& other) const {
+        return (x > other.x) || 
+               (x == other.x && y > other.y) || 
+               (x == other.x && y == other.y && z > other.z);
+    }
+    
+    bool operator>=(const Vec3& other) const {
+        return (x > other.x) || 
+               (x == other.x && y > other.y) || 
+               (x == other.x && y == other.y && z >= other.z);
+    }
+    
+    Vec3 abs() const {
+        return Vec3(std::abs(x), std::abs(y), std::abs(z));
+    }
+    
+    Vec3 floor() const {
+        return Vec3(std::floor(x), std::floor(y), std::floor(z));
+    }
+    
+    Vec3 ceil() const {
+        return Vec3(std::ceil(x), std::ceil(y), std::ceil(z));
+    }
+    
+    Vec3 round() const {
+        return Vec3(std::round(x), std::round(y), std::round(z));
+    }
+    
+    Vec3 min(const Vec3& other) const {
+        return Vec3(std::min(x, other.x), std::min(y, other.y), std::min(z, other.z));
+    }
+    
+    Vec3 max(const Vec3& other) const {
+        return Vec3(std::max(x, other.x), std::max(y, other.y), std::max(z, other.z));
+    }
+    
+    Vec3 clamp(const Vec3& minVal, const Vec3& maxVal) const {
+        return Vec3(
+            std::clamp(x, minVal.x, maxVal.x),
+            std::clamp(y, minVal.y, maxVal.y),
+            std::clamp(z, minVal.z, maxVal.z)
+        );
+    }
+    
+    Vec3 clamp(float minVal, float maxVal) const {
+        return Vec3(
+            std::clamp(x, minVal, maxVal),
+            std::clamp(y, minVal, maxVal),
+            std::clamp(z, minVal, maxVal)
+        );
+    }
+    
+    bool isZero(float epsilon = 1e-10f) const {
+        return std::abs(x) < epsilon && std::abs(y) < epsilon && std::abs(z) < epsilon;
+    }
+    
+    bool equals(const Vec3& other, float epsilon = 1e-10f) const {
+        return std::abs(x - other.x) < epsilon && 
+               std::abs(y - other.y) < epsilon &&
+               std::abs(z - other.z) < epsilon;
+    }
+    
+    friend Vec3 operator+(float scalar, const Vec3& vec) {
+        return Vec3(scalar + vec.x, scalar + vec.y, scalar + vec.z);
+    }
+    
+    friend Vec3 operator-(float scalar, const Vec3& vec) {
+        return Vec3(scalar - vec.x, scalar - vec.y, scalar - vec.z);
+    }
+    
+    friend Vec3 operator*(float scalar, const Vec3& vec) {
+        return Vec3(scalar * vec.x, scalar * vec.y, scalar * vec.z);
+    }
+    
+    friend Vec3 operator/(float scalar, const Vec3& vec) {
+        return Vec3(scalar / vec.x, scalar / vec.y, scalar / vec.z);
+    }
+
+    Vec3 reflect(const Vec3& normal) const {
+        return *this - 2.0f * this->dot(normal) * normal;
+    }
+    
+    Vec3 lerp(const Vec3& other, float t) const {
+        t = std::clamp(t, 0.0f, 1.0f);
+        return *this + (other - *this) * t;
+    }
+    
+    Vec3 slerp(const Vec3& other, float t) const {
+        t = std::clamp(t, 0.0f, 1.0f);
+        float dot = this->dot(other);
+        dot = std::clamp(dot, -1.0f, 1.0f);
+        
+        float theta = std::acos(dot) * t;
+        Vec3 relative = other - *this * dot;
+        relative = relative.normalized();
+        
+        return (*this * std::cos(theta)) + (relative * std::sin(theta));
+    }
+    
+    Vec3 rotateX(float angle) const {
+        float cosA = std::cos(angle);
+        float sinA = std::sin(angle);
+        return Vec3(x, y * cosA - z * sinA, y * sinA + z * cosA);
+    }
+    
+    Vec3 rotateY(float angle) const {
+        float cosA = std::cos(angle);
+        float sinA = std::sin(angle);
+        return Vec3(x * cosA + z * sinA, y, -x * sinA + z * cosA);
+    }
+    
+    Vec3 rotateZ(float angle) const {
+        float cosA = std::cos(angle);
+        float sinA = std::sin(angle);
+        return Vec3(x * cosA - y * sinA, x * sinA + y * cosA, z);
+    }
+    
+    float angleTo(const Vec3& other) const {
+        return std::acos(this->dot(other) / (this->length() * other.length()));
+    }
+    
+    float& operator[](int index) {
+        return (&x)[index];
+    }
+    
+    const float& operator[](int index) const {
+        return (&x)[index];
+    }
+    
+    std::string toString() const {
+        return "(" + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(z) + ")";
+    }
+};
+
+inline std::ostream& operator<<(std::ostream& os, const Vec3& vec) {
+    os << vec.toString();
+    return os;
+}
+
+namespace std {
+    template<>
+    struct hash<Vec3> {
+        size_t operator()(const Vec3& v) const {
+            return hash<float>()(v.x) ^ (hash<float>()(v.y) << 1) ^ (hash<float>()(v.z) << 2);
+        }
+    };
+}
+
+#endif

util/vec4.hpp

@@ -0,0 +1,384 @@
+#ifndef VEC4_HPP
+#define VEC4_HPP
+
+#include "vec3.hpp"
+#include <cmath>
+#include <algorithm>
+#include <string>
+#include <ostream>
+#include <cstdint>
+
+class Vec4 {
+public:
+    union {
+        struct { float x, y, z, w; };
+        struct { float r, g, b, a; };
+        struct { float s, t, p, q; }; // For texture coordinates
+    };
+    
+    // Constructors
+    Vec4() : x(0), y(0), z(0), w(0) {}
+    Vec4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}
+    Vec4(float scalar) : x(scalar), y(scalar), z(scalar), w(scalar) {}
+    
+    Vec4(const Vec3& rgb, float w = 1.0f) : x(rgb.x), y(rgb.y), z(rgb.z), w(w) {}
+    static Vec4 RGB(float r, float g, float b, float a = 1.0f) { return Vec4(r, g, b, a); }
+    static Vec4 RGBA(float r, float g, float b, float a) { return Vec4(r, g, b, a); }
+    
+    
+    Vec4 operator+(const Vec4& other) const {
+        return Vec4(x + other.x, y + other.y, z + other.z, w + other.w);
+    }
+    
+    Vec4 operator-(const Vec4& other) const {
+        return Vec4(x - other.x, y - other.y, z - other.z, w - other.w);
+    }
+    
+    Vec4 operator*(const Vec4& other) const {
+        return Vec4(x * other.x, y * other.y, z * other.z, w * other.w);
+    }
+    
+    Vec4 operator/(const Vec4& other) const {
+        return Vec4(x / other.x, y / other.y, z / other.z, w / other.w);
+    }
+
+    Vec4 operator+(float scalar) const {
+        return Vec4(x + scalar, y + scalar, z + scalar, w + scalar);
+    }
+
+    Vec4 operator-(float scalar) const {
+        return Vec4(x - scalar, y - scalar, z - scalar, w - scalar);
+    }
+    
+    Vec4 operator-() const {
+        return Vec4(-x, -y, -z, -w);
+    }
+
+    Vec4 operator*(float scalar) const {
+        return Vec4(x * scalar, y * scalar, z * scalar, w * scalar);
+    }
+
+    Vec4 operator/(float scalar) const {
+        return Vec4(x / scalar, y / scalar, z / scalar, w / scalar);
+    }
+
+    Vec4& operator=(float scalar) {
+        x = y = z = w = scalar;
+        return *this;
+    }
+    
+    Vec4& operator+=(const Vec4& other) {
+        x += other.x;
+        y += other.y;
+        z += other.z;
+        w += other.w;
+        return *this;
+    }
+    
+    Vec4& operator-=(const Vec4& other) {
+        x -= other.x;
+        y -= other.y;
+        z -= other.z;
+        w -= other.w;
+        return *this;
+    }
+    
+    Vec4& operator*=(const Vec4& other) {
+        x *= other.x;
+        y *= other.y;
+        z *= other.z;
+        w *= other.w;
+        return *this;
+    }
+    
+    Vec4& operator/=(const Vec4& other) {
+        x /= other.x;
+        y /= other.y;
+        z /= other.z;
+        w /= other.w;
+        return *this;
+    }
+    
+    Vec4& operator+=(float scalar) {
+        x += scalar;
+        y += scalar;
+        z += scalar;
+        w += scalar;
+        return *this;
+    }
+    
+    Vec4& operator-=(float scalar) {
+        x -= scalar;
+        y -= scalar;
+        z -= scalar;
+        w -= scalar;
+        return *this;
+    }
+    
+    Vec4& operator*=(float scalar) {
+        x *= scalar;
+        y *= scalar;
+        z *= scalar;
+        w *= scalar;
+        return *this;
+    }
+    
+    Vec4& operator/=(float scalar) {
+        x /= scalar;
+        y /= scalar;
+        z /= scalar;
+        w /= scalar;
+        return *this;
+    }
+
+    float dot(const Vec4& other) const {
+        return x * other.x + y * other.y + z * other.z + w * other.w;
+    }
+    
+    // 4D cross product (returns vector perpendicular to 3 given vectors in 4D space)
+    Vec4 cross(const Vec4& v1, const Vec4& v2, const Vec4& v3) const {
+        float a = v1.y * (v2.z * v3.w - v2.w * v3.z) -
+                  v1.z * (v2.y * v3.w - v2.w * v3.y) +
+                  v1.w * (v2.y * v3.z - v2.z * v3.y);
+                  
+        float b = -v1.x * (v2.z * v3.w - v2.w * v3.z) +
+                   v1.z * (v2.x * v3.w - v2.w * v3.x) -
+                   v1.w * (v2.x * v3.z - v2.z * v3.x);
+                   
+        float c = v1.x * (v2.y * v3.w - v2.w * v3.y) -
+                  v1.y * (v2.x * v3.w - v2.w * v3.x) +
+                  v1.w * (v2.x * v3.y - v2.y * v3.x);
+                  
+        float d = -v1.x * (v2.y * v3.z - v2.z * v3.y) +
+                   v1.y * (v2.x * v3.z - v2.z * v3.x) -
+                   v1.z * (v2.x * v3.y - v2.y * v3.x);
+        
+        return Vec4(a, b, c, d);
+    }
+
+    float length() const {
+        return std::sqrt(x * x + y * y + z * z + w * w);
+    }
+    
+    float lengthSquared() const {
+        return x * x + y * y + z * z + w * w;
+    }
+    
+    float distance(const Vec4& other) const {
+        return (*this - other).length();
+    }
+    
+    float distanceSquared(const Vec4& other) const {
+        Vec4 diff = *this - other;
+        return diff.x * diff.x + diff.y * diff.y + diff.z * diff.z + diff.w * diff.w;
+    }
+
+    Vec4 normalized() const {
+        float len = length();
+        if (len > 0) {
+            return *this / len;
+        }
+        return *this;
+    }
+    
+    // Homogeneous normalization (divide by w)
+    Vec4 homogenized() const {
+        if (w != 0.0f) {
+            return Vec4(x / w, y / w, z / w, 1.0f);
+        }
+        return *this;
+    }
+    
+    // Clamp values between 0 and 1
+    Vec4 clamped() const {
+        return Vec4(
+            std::clamp(r, 0.0f, 1.0f),
+            std::clamp(g, 0.0f, 1.0f),
+            std::clamp(b, 0.0f, 1.0f),
+            std::clamp(a, 0.0f, 1.0f)
+        );
+    }
+    
+    // Convert to Vec3 (ignoring alpha)
+    Vec3 toVec3() const {
+        return Vec3(r, g, b);
+    }
+    
+    // Convert to 8-bit color values
+    void toUint8(uint8_t& red, uint8_t& green, uint8_t& blue, uint8_t& alpha) const {
+        red = static_cast<uint8_t>(std::clamp(r, 0.0f, 1.0f) * 255);
+        green = static_cast<uint8_t>(std::clamp(g, 0.0f, 1.0f) * 255);
+        blue = static_cast<uint8_t>(std::clamp(b, 0.0f, 1.0f) * 255);
+        alpha = static_cast<uint8_t>(std::clamp(a, 0.0f, 1.0f) * 255);
+    }
+    
+    void toUint8(uint8_t& red, uint8_t& green, uint8_t& blue) const {
+        red = static_cast<uint8_t>(std::clamp(r, 0.0f, 1.0f) * 255);
+        green = static_cast<uint8_t>(std::clamp(g, 0.0f, 1.0f) * 255);
+        blue = static_cast<uint8_t>(std::clamp(b, 0.0f, 1.0f) * 255);
+    }
+    // Get XYZ components as Vec3
+    class Vec3 xyz() const;
+    
+    // Get RGB components as Vec3
+    class Vec3 rgb() const;
+    
+    bool operator==(const Vec4& other) const {
+        return x == other.x && y == other.y && z == other.z && w == other.w;
+    }
+    
+    bool operator!=(const Vec4& other) const {
+        return x != other.x || y != other.y || z != other.z || w != other.w;
+    }
+    
+    bool operator<(const Vec4& other) const {
+        return (x < other.x) || 
+               (x == other.x && y < other.y) || 
+               (x == other.x && y == other.y && z < other.z) ||
+               (x == other.x && y == other.y && z == other.z && w < other.w);
+    }
+    
+    bool operator<=(const Vec4& other) const {
+        return (x < other.x) || 
+               (x == other.x && y < other.y) || 
+               (x == other.x && y == other.y && z < other.z) ||
+               (x == other.x && y == other.y && z == other.z && w <= other.w);
+    }
+    
+    bool operator>(const Vec4& other) const {
+        return (x > other.x) || 
+               (x == other.x && y > other.y) || 
+               (x == other.x && y == other.y && z > other.z) ||
+               (x == other.x && y == other.y && z == other.z && w > other.w);
+    }
+    
+    bool operator>=(const Vec4& other) const {
+        return (x > other.x) || 
+               (x == other.x && y > other.y) || 
+               (x == other.x && y == other.y && z > other.z) ||
+               (x == other.x && y == other.y && z == other.z && w >= other.w);
+    }
+    
+    Vec4 abs() const {
+        return Vec4(std::abs(x), std::abs(y), std::abs(z), std::abs(w));
+    }
+    
+    Vec4 floor() const {
+        return Vec4(std::floor(x), std::floor(y), std::floor(z), std::floor(w));
+    }
+    
+    Vec4 ceil() const {
+        return Vec4(std::ceil(x), std::ceil(y), std::ceil(z), std::ceil(w));
+    }
+    
+    Vec4 round() const {
+        return Vec4(std::round(x), std::round(y), std::round(z), std::round(w));
+    }
+    
+    Vec4 min(const Vec4& other) const {
+        return Vec4(std::min(x, other.x), std::min(y, other.y), 
+                   std::min(z, other.z), std::min(w, other.w));
+    }
+    
+    Vec4 max(const Vec4& other) const {
+        return Vec4(std::max(x, other.x), std::max(y, other.y), 
+                   std::max(z, other.z), std::max(w, other.w));
+    }
+    
+    Vec4 clamp(float minVal, float maxVal) const {
+        return Vec4(
+            std::clamp(x, minVal, maxVal),
+            std::clamp(y, minVal, maxVal),
+            std::clamp(z, minVal, maxVal),
+            std::clamp(w, minVal, maxVal)
+        );
+    }
+    
+    // Color-specific clamping (clamps RGB between 0 and 1)
+    Vec4 clampColor() const {
+        return Vec4(
+            std::clamp(r, 0.0f, 1.0f),
+            std::clamp(g, 0.0f, 1.0f),
+            std::clamp(b, 0.0f, 1.0f),
+            std::clamp(a, 0.0f, 1.0f)
+        );
+    }
+    
+    bool isZero(float epsilon = 1e-10f) const {
+        return std::abs(x) < epsilon && std::abs(y) < epsilon && 
+               std::abs(z) < epsilon && std::abs(w) < epsilon;
+    }
+    
+    bool equals(const Vec4& other, float epsilon = 1e-10f) const {
+        return std::abs(x - other.x) < epsilon && 
+               std::abs(y - other.y) < epsilon &&
+               std::abs(z - other.z) < epsilon &&
+               std::abs(w - other.w) < epsilon;
+    }
+    
+    friend Vec4 operator+(float scalar, const Vec4& vec) {
+        return Vec4(scalar + vec.x, scalar + vec.y, scalar + vec.z, scalar + vec.w);
+    }
+    
+    friend Vec4 operator-(float scalar, const Vec4& vec) {
+        return Vec4(scalar - vec.x, scalar - vec.y, scalar - vec.z, scalar - vec.w);
+    }
+    
+    friend Vec4 operator*(float scalar, const Vec4& vec) {
+        return Vec4(scalar * vec.x, scalar * vec.y, scalar * vec.z, scalar * vec.w);
+    }
+    
+    friend Vec4 operator/(float scalar, const Vec4& vec) {
+        return Vec4(scalar / vec.x, scalar / vec.y, scalar / vec.z, scalar / vec.w);
+    }
+
+    Vec4 lerp(const Vec4& other, float t) const {
+        t = std::clamp(t, 0.0f, 1.0f);
+        return *this + (other - *this) * t;
+    }
+    
+    // Convert to grayscale using standard RGB weights
+    float grayscale() const {
+        return r * 0.299f + g * 0.587f + b * 0.114f;
+    }
+    
+    // Color inversion (1.0 - color)
+    Vec4 inverted() const {
+        return Vec4(1.0f - r, 1.0f - g, 1.0f - b, a);
+    }
+    
+    float& operator[](int index) {
+        return (&x)[index];
+    }
+    
+    const float& operator[](int index) const {
+        return (&x)[index];
+    }
+    
+    std::string toString() const {
+        return "(" + std::to_string(x) + ", " + std::to_string(y) + ", " + 
+               std::to_string(z) + ", " + std::to_string(w) + ")";
+    }
+    
+    std::string toColorString() const {
+        return "RGBA(" + std::to_string(r) + ", " + std::to_string(g) + ", " + 
+               std::to_string(b) + ", " + std::to_string(a) + ")";
+    }
+};
+
+inline std::ostream& operator<<(std::ostream& os, const Vec4& vec) {
+    os << vec.toString();
+    return os;
+}
+
+namespace std {
+    template<>
+    struct hash<Vec4> {
+        size_t operator()(const Vec4& v) const {
+            return hash<float>()(v.x) ^ (hash<float>()(v.y) << 1) ^ 
+                   (hash<float>()(v.z) << 2) ^ (hash<float>()(v.w) << 3);
+        }
+    };
+}
+
+#endif

util/voxelgrid.hpp

@@ -0,0 +1,217 @@
+#ifndef VOXEL_HPP
+#define VOXEL_HPP
+
+#include <vector>
+#include <cmath>
+#include <limits>
+#include <algorithm>
+#include <unordered_map>
+#include <cstdio>
+#include <cstring>
+#include <fstream>
+#include <iostream>
+#include <vector>
+#include <random>
+#include <functional>
+#include <tuple>
+#include "timing_decorator.hpp"
+#include "vec3.hpp"
+#include "vec4.hpp"
+
+class VoxelGrid {
+private:
+    std::unordered_map<Vec3, size_t> positionToIndex;
+    std::vector<Vec3> positions;
+    std::vector<Vec4> colors;
+    std::vector<int> layers;
+    
+    Vec3 gridSize;
+
+public:
+    Vec3 voxelSize;
+    
+    enum LayerType {
+        ATMOSPHERE = 0,
+        CRUST = 1,
+        MANTLE = 2,
+        OUTER_CORE = 3,
+        INNER_CORE = 4,
+        EMPTY = -1
+    };
+    
+    VoxelGrid(const Vec3& size, const Vec3& voxelSize = Vec3(1, 1, 1)) : gridSize(size), voxelSize(voxelSize) {}
+    
+    void addVoxel(const Vec3& position, const Vec4& color) {
+        Vec3 gridPos = worldToGrid(position);
+        
+        auto it = positionToIndex.find(gridPos);
+        if (it == positionToIndex.end()) {
+            size_t index = positions.size();
+            positions.push_back(gridPos);
+            colors.push_back(color);
+            layers.push_back(EMPTY);
+            positionToIndex[gridPos] = index;
+        } else {
+            colors[it->second] = color;
+        }
+    }
+    
+    void addVoxelWithLayer(const Vec3& position, const Vec4& color, int layer) {
+        Vec3 gridPos = worldToGrid(position);
+        
+        auto it = positionToIndex.find(gridPos);
+        if (it == positionToIndex.end()) {
+            size_t index = positions.size();
+            positions.push_back(gridPos);
+            colors.push_back(color);
+            layers.push_back(layer);
+            positionToIndex[gridPos] = index;
+        } else {
+            colors[it->second] = color;
+            layers[it->second] = layer;
+        }
+    }
+    
+    Vec4 getVoxel(const Vec3& position) const {
+        Vec3 gridPos = worldToGrid(position);
+        auto it = positionToIndex.find(gridPos);
+        if (it != positionToIndex.end()) {
+            return colors[it->second];
+        }
+        return Vec4(0, 0, 0, 0);
+    }
+    
+    int getVoxelLayer(const Vec3& position) const {
+        Vec3 gridPos = worldToGrid(position);
+        auto it = positionToIndex.find(gridPos);
+        if (it != positionToIndex.end()) {
+            return layers[it->second];
+        }
+        return EMPTY;
+    }
+    
+    bool isOccupied(const Vec3& position) const {
+        Vec3 gridPos = worldToGrid(position);
+        return positionToIndex.find(gridPos) != positionToIndex.end();
+    }
+    
+    Vec3 worldToGrid(const Vec3& worldPos) const {
+        return (worldPos / voxelSize).floor();
+    }
+    
+    Vec3 gridToWorld(const Vec3& gridPos) const {
+        return gridPos * voxelSize;
+    }
+    
+    const std::vector<Vec3>& getOccupiedPositions() const {
+        return positions;
+    }
+    
+    const std::vector<Vec4>& getColors() const {
+        return colors;
+    }
+    
+    const std::vector<int>& getLayers() const {
+        return layers;
+    }
+    
+    const std::unordered_map<Vec3, size_t>& getPositionToIndexMap() const {
+        return positionToIndex;
+    }
+    
+    const Vec3& getGridSize() const {
+        return gridSize;
+    }
+    
+    const Vec3& getVoxelSize() const {
+        return voxelSize;
+    }
+    
+    void clear() {
+        positions.clear();
+        colors.clear();
+        layers.clear();
+        positionToIndex.clear();
+    }
+    
+    void assignPlanetaryLayers(const Vec3& center = Vec3(0, 0, 0)) {
+        TIME_FUNCTION;
+        printf("Assigning planetary layers...\n");
+        
+        const float atmospherePercent = 0.05f;
+        const float crustPercent = 0.01f;
+        const float mantlePercent = 0.10f;
+        const float outerCorePercent = 0.42f;
+        const float innerCorePercent = 0.42f;
+        
+        float maxDistance = 0.0f;
+        for (const auto& pos : positions) {
+            Vec3 worldPos = gridToWorld(pos);
+            float distance = (worldPos - center).length();
+            maxDistance = std::max(maxDistance, distance);
+        }
+        
+        printf("Maximum distance from center: %.2f\n", maxDistance);
+        
+        const float atmosphereStart = maxDistance * (1.0f - atmospherePercent);
+        const float crustStart = maxDistance * (1.0f - atmospherePercent - crustPercent);
+        const float mantleStart = maxDistance * (1.0f - atmospherePercent - crustPercent - mantlePercent);
+        const float outerCoreStart = maxDistance * (1.0f - atmospherePercent - crustPercent - mantlePercent - outerCorePercent);
+        
+        printf("Layer boundaries:\n");
+        printf("  Atmosphere: %.2f to %.2f\n", atmosphereStart, maxDistance);
+        printf("  Crust: %.2f to %.2f\n", crustStart, atmosphereStart);
+        printf("  Mantle: %.2f to %.2f\n", mantleStart, crustStart);
+        printf("  Outer Core: %.2f to %.2f\n", outerCoreStart, mantleStart);
+        printf("  Inner Core: 0.00 to %.2f\n", outerCoreStart);
+        
+        int atmosphereCount = 0, crustCount = 0, mantleCount = 0, outerCoreCount = 0, innerCoreCount = 0;
+        
+        for (size_t i = 0; i < positions.size(); ++i) {
+            Vec3 worldPos = gridToWorld(positions[i]);
+            float distance = (worldPos - center).length();
+            
+            Vec4 layerColor;
+            int layerType;
+            
+            if (distance >= atmosphereStart) {
+                // Atmosphere - transparent blue
+                layerColor = Vec4(0.2f, 0.4f, 1.0f, 0.3f); // Semi-transparent blue
+                layerType = ATMOSPHERE;
+                atmosphereCount++;
+            } else if (distance >= crustStart) {
+                // Crust - light brown
+                layerColor = Vec4(0.8f, 0.7f, 0.5f, 1.0f); // Light brown
+                layerType = CRUST;
+                crustCount++;
+            } else if (distance >= mantleStart) {
+                // Mantle - reddish brown
+                layerColor = Vec4(0.7f, 0.3f, 0.2f, 1.0f); // Reddish brown
+                layerType = MANTLE;
+                mantleCount++;
+            } else if (distance >= outerCoreStart) {
+                // Outer Core - orange/yellow
+                layerColor = Vec4(1.0f, 0.6f, 0.2f, 1.0f); // Orange
+                layerType = OUTER_CORE;
+                outerCoreCount++;
+            } else {
+                // Inner Core - bright yellow
+                layerColor = Vec4(1.0f, 0.9f, 0.1f, 1.0f); // Bright yellow
+                layerType = INNER_CORE;
+                innerCoreCount++;
+            }
+            
+            colors[i] = layerColor;
+            layers[i] = layerType;
+        }
+        
+        printf("Layer distribution:\n");
+        printf("  Atmosphere: %d voxels (%.1f%%)\n", atmosphereCount, (atmosphereCount * 100.0f) / positions.size());
+        printf("  Crust: %d voxels (%.1f%%)\n", crustCount, (crustCount * 100.0f) / positions.size());
+        printf("  Mantle: %d voxels (%.1f%%)\n", mantleCount, (mantleCount * 100.0f) / positions.size());
+        printf("  Outer Core: %d voxels (%.1f%%)\n", outerCoreCount, (outerCoreCount * 100.0f) / positions.size());
+        printf("  Inner Core: %d voxels (%.1f%%)\n", innerCoreCount, (innerCoreCount * 100.0f) / positions.size());
+    }
+};
+
+#endif