Physics/RigidBody.h

/*
 * RigidBody.h
 *
 *  Created on: 2010-11-10
 *      Author: krejziwan
 */

#ifndef RIGIDBODY_H_
#define RIGIDBODY_H_

#include "../Common/Vector.h"
#include <vector>
#include "../Common/Constants.h"


class RigidBody {
protected:
        Vector position, velocity, rotation;
        real inverseMass;
        real radius;
        bool awake;
        real motion;//motion = velocity*velocity+rotation*rotation
        std::vector<RigidBody *> neighbors;
        int number;
public:
        RigidBody() {motion=0.0;awake = false;};
        real calculateEnergy() const {
                return velocity*velocity;//+rotation*rotation;
        }
        void integrate(real duration);
        bool isAwake() const {
                return awake;
        }
        void setAwake(const bool b) {
                this->awake = b;
                if(!b) {
                        velocity.reset();
                        rotation.reset();
                        motion = 0;
                }
        }
        void resolveCollision(const RigidBody & body) {
                //TODO
                real d = calculateSquaredDistance(body);

                if(d*d<4*radius*radius||(d*d)) {//we have a collision
                        Vector normal = calculateNormalVector(body);

                }
        }

        Vector calculateVelChange(real duration)const ;
        real calculateSquaredDistance(const RigidBody & body) const {
                return (body.position-this->position).squareMagnitude();
        }

        Vector calculateNormalVector(const RigidBody & body) const {
                Vector mid = (this->position);
                mid -= body.position;
                return (real)1.0/mid.magnitude()*mid;
        }

        Vector getVelocity() const {
                return velocity;
        }

        Vector getPosition() const {
                return position;
        }

        int getNumber() const {
                return number;
        }

        void setNumber(int _num) {
                number = _num;
        }
        void updatePosition(const Vector& v) {
                position += v;
        }

        void updateVelocity(const Vector& v) {
                velocity +=v;
//              if(velocity.squareMagnitude()<0.02)
//                      setAwake(false);
        }
        void setVelocity(const Vector&v) {
                velocity = v;
//              if(velocity.squareMagnitude()<0.02)
//                      setAwake(false);
        }

        void findNeighbors(std::vector<RigidBody *>& bodies) {
                neighbors.clear();
                for(std::vector<RigidBody *>::iterator it = bodies.begin(); it!=bodies.end();++it) {
                        if(*it==this)
                                continue;
                        if(((*it)->getPosition()-position).squareMagnitude()<=1.02*4*BALLRADIUS*BALLRADIUS)
                                neighbors.push_back(*it);
                }
//              std::cout << "Body: " << number << " Neighs: ";
//              for(std::vector<RigidBody *>::iterator it = neighbors.begin();it!=neighbors.end();++it)
//                      std::cout << (*it)->getNumber() << ' ';
//              std::cout << '\n';
        }

        bool isPartOfGroup() const {
                return neighbors.size() > 1;
        }

        RigidBody * findLeft(Vector direction) {
                RigidBody * founded = NULL;
                Vector relPos;
                real sin,cos;
                //in order to find first ball on right we need to ensure that cos > 0 (which means that tested ball is actually in front of moving ball
                //and sin
                real max = 1.0;
                for(std::vector<RigidBody *>::iterator it = neighbors.begin();it!=neighbors.end();++it) {
                        relPos = (*it)->getPosition()-position;
                        cos = relPos.scalarProduct(direction);
                        if(cos>0) {
                                sin = direction.getX()*relPos.unit().getZ()-direction.getZ()*relPos.unit().getX();
                                if(sin>=0.0 && sin < max) {
                                        founded = *it;
                                        max = sin;
                                }
                        }
                }
                return founded;
        }

        RigidBody * findRight(Vector direction) {
                RigidBody * founded = NULL;
                Vector relPos;
                real sin,cos;
                //in order to find first ball on right we need to ensure that cos > 0 (which means that tested ball is actually in front of moving ball
                //and sin
                real max = -1.0;
                for(std::vector<RigidBody *>::iterator it = neighbors.begin();it!=neighbors.end();++it) {
                        relPos = (*it)->getPosition()-position;
                        cos = relPos.scalarProduct(direction);
                        if(cos>0) {
                                sin = direction.getX()*relPos.unit().getZ()-direction.getZ()*relPos.unit().getX();
                                if(sin<=0.0 && sin > max) {
                                        founded = *it;
                                        max = sin;
                                }
                        }
                }
                return founded;
        }
};


#endif /* RIGIDBODY_H_ */