///grabbed
    void parent(CommonRigidBodyBase* bullet_scene, int id, quaternion base, quaternion current_parent, vec3f position_offset, vec3f ideal_position_offset, bool slide = false)
    {
        parent_id = id;

        base_diff = current_parent.get_difference(base);

        should_hand_collide = false;

        make_kinematic(bullet_scene);

        mat3f hand_rot = current_parent.get_rotation_matrix();

        offset = hand_rot.transp() * position_offset;
        ideal_offset = hand_rot.transp() * ideal_position_offset;

        is_parented = true;

        if(slide && can_slide)
        {
            slide_towards_parent = true;
            slide_timer = 0;
            slide_parent_init = false;
        }

        //printf("%f %f %f %f base\n", base_diff.x(), base_diff.y(), base_diff.z(), base_diff.w());
    }

	quaternion exp( const quaternion& lhs ){
		float alpha = lhs.norm();
		vec3 v = normalize3( lhs.comps().xyz() );

		float sin_alpha = 0.0f;
		float cos_alpha = 0.0f;
		sincos(alpha, sin_alpha, cos_alpha);

		vec4 q_v;
		q_v.w( cos_alpha );
		q_v.xyz( sin_alpha * v );

		return quaternion(q_v);
	}

void quaternion::Intermediate(const quaternion &rkQ0, const quaternion &rkQ1, const quaternion &rkQ2, quaternion &rkA, quaternion &rkB)
{
    // assert:  q0, q1, q2 are unit quaternions

    quaternion kQ0inv = rkQ0.UnitInverse();
    quaternion kQ1inv = rkQ1.UnitInverse();
    quaternion rkP0 = kQ0inv*rkQ1;
    quaternion rkP1 = kQ1inv*rkQ2;
    quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
    quaternion kMinusArg = -kArg;

    rkA = rkQ1*kArg.Exp();
    rkB = rkQ1*kMinusArg.Exp();
}

// Uses the given angular velocity and time interval to calculate
// a rotation and applies that rotation to the given quaternion.
// w is angular velocity in radians per second.
// dt is the time.
void ahrs::rotate(quaternion& rotation, const vector& w, float dt)
{
    // Multiply by first order approximation of the
    // quaternion representing this rotation.
    rotation *= quaternion(1, w(0)*dt/2, w(1)*dt/2, w(2)*dt/2);
    rotation.normalize();
}

Foam::triad::triad(const quaternion& q)
{
    tensor Rt(q.R().T());
    x() = Rt.x();
    y() = Rt.y();
    z() = Rt.z();
}

quaternion slerp(const quaternion& p1, const quaternion& q1, float t) {
	quaternion q = q1.normalise();
	quaternion p = p1.normalise();

	float epsilon = 0.0001;
	if (dotproduct(p, q) < 0) {
		q = q * -1;
	}
	float dpq = dotproduct(p, q);
	if ((1.0 - dpq) > epsilon) {
		float w = acos(dpq);
		return ((sin((1 - t) * w) * p) + (sin(t * w) * q)) / sin(w);
	} else {
		return (1 - t) * p + t * q;
	}
}

void ahrs::fuse_default(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    vector correction = vector(0, 0, 0);

    if (abs(acceleration.norm() - 1) <= 0.3)
    {
        // The magnitude of acceleration is close to 1 g, so
        // it might be pointing up and we can do drift correction.

        const float correction_strength = 1;

        matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);
        matrix rotationMatrix = rotation.toRotationMatrix();

        correction = (
            rotationCompass.row(0).cross(rotationMatrix.row(0)) +
            rotationCompass.row(1).cross(rotationMatrix.row(1)) +
            rotationCompass.row(2).cross(rotationMatrix.row(2))
          ) * correction_strength;

    }

    rotate(rotation, angular_velocity + correction, dt);
}

        void clipmap_ring::update(const vector & eye_dir_in_object_space)
        {
            // get quaternion rotations for clipmap
            double view_phi = ::acos(eye_dir_in_object_space[2]);
            double view_theta = ::atan2(eye_dir_in_object_space[1], eye_dir_in_object_space[0]);

            if ( level && !( (::fabs(view_phi - old_view_phi) > VIEW_EPSILON) || (::fabs(view_theta - old_view_theta) > VIEW_EPSILON) ) )
            {
                return;
            }

            old_view_phi = view_phi;
            old_view_theta = view_theta;

            roty = quaternion(vector::Y_AXIS, view_phi);
            roty_conj = roty.conjugate();

            rotz = quaternion(vector::Z_AXIS, view_theta);
            rotz_conj = rotz.conjugate();
          
            // generate vertices & indices
            generate_vertices();
            generate_indices();

            // create buffers if necessary
            if (!vbuf_id)
            {
                glGenBuffers(1, (GLuint *) &vbuf_id);
                if (!vbuf_id)
                    throw opengl_exception(__FILE__, __LINE__, L"clipmap_ring unable to generate buffer id: ");
            }

            if (!ibuf_id)
            {
                glGenBuffers(1, (GLuint *) &ibuf_id);
                if (!ibuf_id)
                    throw opengl_exception(__FILE__, __LINE__, L"clipmap_ring unable to generate buffer id");
            }

            // update buffers
            glBindBuffer(GL_ARRAY_BUFFER, vbuf_id);
            glBufferData(GL_ARRAY_BUFFER, sizeof(float) * vbuf.size(), vbuf.ptr(), GL_STREAM_DRAW);

            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ibuf_id);
            glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(unsigned int) * ibuf.size(), ibuf.ptr(), GL_STREAM_DRAW);
        } // clipmap_ring::update()

	quaternion log( const quaternion& lhs ){
		float alpha = acos(lhs.w);
		if( equal( std::abs(alpha), 1.0f ) ){
			return lhs;
		}
		vec3 new_v = normalize3( lhs.comps().xyz() );
		return quaternion(alpha*new_v[0], alpha*new_v[1], alpha*new_v[2], 0.0f);
	}

void Foam::transform
(
    vectorField& rtf,
    const quaternion& q,
    const vectorField& tf
)
{
    tensor t = q.R();
    TFOR_ALL_F_OP_FUNC_S_F(vector, rtf, =, transform, tensor, t, vector, tf)
}

	quaternion slerp( const quaternion& src, const quaternion& dest, float t )
	{
		float cos_omega = dot_prod4(src.comps(), dest.comps());
		vec4 near_dest_v = dest.comps() * sign(cos_omega);
		cos_omega = abs(cos_omega);

		float k0(0.0f), k1(0.0f);
		if( equal(cos_omega, 1.0f) ){
			k0 = 1.0f - t;
			k1 = t;
		}else{
			float sin_omega = sqrt(1.0f - cos_omega*cos_omega);
			float omega = atan2(sin_omega, cos_omega);
			float inv_sin_omega = 1.0f / sin_omega;
			k0 = sin( (1.0f - t) * omega ) * inv_sin_omega;
			k1 = sin( t * omega ) * inv_sin_omega;
		}

		return quaternion( src.comps() * k0 + near_dest_v * k1);
	}

    void set_trans(cl_float4 clpos, quaternion m)
    {
        mat3f mat_diff = base_diff.get_rotation_matrix();

        mat3f current_hand = m.get_rotation_matrix();
        mat3f my_rot = current_hand * mat_diff;

        quaternion n;
        n.load_from_matrix(my_rot);


        vec3f absolute_pos = {clpos.x, clpos.y, clpos.z};

        ///current hand does not take into account the rotation offset when grabbing
        ///ie we'll double rotate
        vec3f offset_rot = current_hand * offset;

        vec3f pos = absolute_pos + offset_rot;

        btTransform newTrans;

        //rigid_body->getMotionState()->getWorldTransform(newTrans);

        newTrans.setOrigin(btVector3(pos.v[0], pos.v[1], pos.v[2]));
        newTrans.setRotation(btQuaternion(n.x(), n.y(), n.z(), n.w()));

        rigid_body->getMotionState()->setWorldTransform(newTrans);
        //rigid_body->setInterpolationWorldTransform(newTrans);

        //if(ctr)
        //    ctr->set_pos(conv_implicit<cl_float4>(pos));

        slide_parent_init = true;
        slide_saved_parent = absolute_pos;

        remote_pos = pos;
        remote_rot = n;

        kinematic_old = kinematic_current;
        kinematic_current = xyzf_to_vec(rigid_body->getWorldTransform().getOrigin());
    }

quaternion quaternion::nlerp(real fT, const quaternion &rkP, const quaternion &rkQ, bool shortestPath)
{
    quaternion result;
    real fCos = rkP.Dot(rkQ);
    if (fCos < 0.0f && shortestPath)
    {
        result = rkP + fT * ((-rkQ) - rkP);
    }
    else
    {
        result = rkP + fT * (rkQ - rkP);
    }
    result.normalise();
    return result;
}

quaternion quaternion::SlerpExtraSpins(real fT, const quaternion& rkP, const quaternion& rkQ, int iExtraSpins)
{
    real fCos = rkP.Dot(rkQ);
    real fAngle (math::acos(fCos) );

    if (math::abs(fAngle) < msEpsilon )
       return rkP;

    real fSin = math::sin(fAngle);
    real fPhase ( math::pi*iExtraSpins*fT );
    real fInvSin = 1.0f/fSin;
    real fCoeff0 = math::sin((1.0f-fT)*fAngle - fPhase)*fInvSin;
    real fCoeff1 = math::sin(fT*fAngle + fPhase)*fInvSin;
    return fCoeff0*rkP + fCoeff1*rkQ;
}

quaternion quaternion::Slerp(real fT, const quaternion &rkP, const quaternion &rkQ, bool shortestPath)
{
    real fCos = rkP.Dot(rkQ);
    quaternion rkT;

    // Do we need to invert rotation?
    if (fCos < 0.0f && shortestPath)
    {
        fCos = -fCos;
        rkT = -rkQ;
    }
    else
    {
        rkT = rkQ;
    }

    if (math::abs(fCos) < 1 - msEpsilon)
    {
        // Standard case (slerp)
        real fSin = math::sqrt(1 - fCos*fCos);
        real fAngle = math::atan2(fSin, fCos);
        real fInvSin = 1.0f / fSin;
        real fCoeff0 = math::sin((1.0f - fT) * fAngle) * fInvSin;
        real fCoeff1 = math::sin(fT * fAngle) * fInvSin;
        return fCoeff0 * rkP + fCoeff1 * rkT;
    }
    else
    {
        // There are two situations:
        // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
        //    interpolation safely.
        // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
        //    are an infinite number of possibilities interpolation. but we haven't
        //    have method to fix this case, so just use linear interpolation here.
        quaternion t = (1.0f - fT) * rkP + fT * rkT;
        // taking the complement requires renormalisation
        t.normalise();
        return t;
    }
}

void matrix4x4::setOrientation( quaternion in )
    {
    jAssert( fcmp( in.length(), 1 ) );

    JFLOAT xx = in.x() * in.x();
    JFLOAT xy = in.x() * in.y();
    JFLOAT xz = in.x() * in.z();
    JFLOAT xw = in.x() * in.w();

    JFLOAT yy = in.y() * in.y();
    JFLOAT yz = in.y() * in.z();
    JFLOAT yw = in.y() * in.w();

    JFLOAT zz = in.z() * in.z();
    JFLOAT zw = in.z() * in.w();


#ifndef USEALTERNATIVEQUATTOMATRIX
    JFLOAT ww = in.w() * in.w();

    RM(0,0) = xx - yy - zz + ww;
    RM(1,0) = 2 * ( xy - zw );
    RM(2,0) = 2 * ( xz + yw );

    RM(0,1) = 2 * ( xy + zw );
    RM(1,1) = -xx + yy - zz + ww;
    RM(2,1) = 2 * ( yz - xw );

    RM(0,2) = 2 * ( xz - yw );
    RM(1,2) = 2 * ( yz + xw );
    RM(2,2) = -xx - yy + zz + ww;
#else
    RM(0,0) = 1.0 - 2.0 * ( yy + zz );
    RM(0,1) = 2 * ( xy + zw );
    RM(0,2) = 2 * ( xz - yw );

    RM(1,0) = 2 * ( xy - zw );
    RM(1,1) = 1 - 2 * ( xx + zz );
    RM(1,2) = 2 * ( yz + xw );

    RM(2,0) = 2 * ( xz + yw );
    RM(2,1) = 2 * ( yz - xw );
    RM(2,2) = 1 - 2 * ( xx + yy );
#endif
    }

void Camera::rotate(const quaternion& q)
{
    _modelViewMatrix *= q.toMatrix();
    modelViewUpdated();
}

 quaternion(quaternion const& other):container(other.data()){}

quaternion operator-(quaternion const& q)
{
    return quaternion(-q.x(),-q.y(),-q.z(),-q.w());
}

void quaternion::multiply(quaternion q) {
	float w_prime = w * q.get_w() - x * q.get_x() - y * q.get_y()
			- z * q.get_z();
	float x_prime = x * q.get_w() + w * q.get_x() + y * q.get_z()
			- z * q.get_y();
	float y_prime = y * q.get_w() + w * q.get_y() + z * q.get_x()
			- x * q.get_z();
	float z_prime = z * q.get_w() + w * q.get_z() + x * q.get_y()
			- y * q.get_x();
	this->x = x_prime;
	this->y = y_prime;
	this->z = z_prime;
	this->w = w_prime;
}