/*
 * Copyright (c) 2008-2009 Bill Whitacre http://rampancy.g0dsoft.com
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
*/

#include "constraint_pivot.h"

static rat_constraint_class rat_pivot_class=
{
	rat_constraint_pivot_destroy,
	rat_constraint_pivot_prime,
	rat_constraint_pivot_step
};

rat_constraint *rat_constraint_pivot_create(rat_body *body_a,rat_body *body_b,vector2 anch)
{
	rat_constraint_pivot *pivot=(rat_constraint_pivot *)rat_alloc(sizeof(rat_constraint_pivot));
	memset(pivot,0,sizeof(rat_constraint_pivot));

	pivot->type=rat_constraint_type_pivot;
	pivot->clas=&rat_pivot_class;

	pivot->body_a=body_a;
	pivot->anch_a=vector2_world_to_local(anch,rat_hull_get_pos(body_a->hull),rat_hull_get_angle(body_a->hull));

	pivot->body_b=body_b;
	pivot->anch_b=vector2_world_to_local(anch,rat_hull_get_pos(body_b->hull),rat_hull_get_angle(body_b->hull));

	if (body_a) rat_body_add_constraint(body_a,(rat_constraint *)pivot);
	if (body_b) rat_body_add_constraint(body_b,(rat_constraint *)pivot);
	
	return (rat_constraint *)pivot;
}

void rat_constraint_pivot_destroy(rat_constraint *constraint)
{
	rat_dealloc((void *)constraint);
}

void rat_constraint_pivot_prime(rat_constraint *constraint,rat_real bias_coef)
{
	rat_constraint_pivot *pivot=(rat_constraint_pivot *)constraint;
	vector2 delta,pos_a,pos_b;
	rat_real angle_a,angle_b;

	// mass matrix
	rat_real mass00,mass01,mass10,mass11,mass_sum,norm_xy_sq,inv_determ;

	pos_a=rat_hull_get_pos(pivot->body_a->hull);
	pos_b=rat_hull_get_pos(pivot->body_b->hull);
	
	angle_a=rat_hull_get_angle(pivot->body_a->hull);
	angle_b=rat_hull_get_angle(pivot->body_b->hull);
	
	pivot->world_anch_a=vector2_getrotated(pivot->anch_a,angle_a);
	pivot->world_anch_b=vector2_getrotated(pivot->anch_b,angle_b);

	// calculate the mass matrix
	mass_sum=pivot->body_a->inv_mass+pivot->body_b->inv_mass;
	mass00=mass_sum; mass01=0.0;
	mass10=0.0; mass11=mass_sum;

	norm_xy_sq=-(pivot->world_anch_a.x*pivot->world_anch_a.y*pivot->body_a->inv_moi);
	mass00+=pivot->world_anch_a.y*pivot->world_anch_a.y*pivot->body_a->inv_moi; mass01+=norm_xy_sq;
	mass10+=norm_xy_sq; mass11+=pivot->world_anch_a.x*pivot->world_anch_a.x*pivot->body_a->inv_moi;

	norm_xy_sq=-(pivot->world_anch_b.x*pivot->world_anch_b.y*pivot->body_b->inv_moi);
	mass00+=pivot->world_anch_b.y*pivot->world_anch_b.y*pivot->body_b->inv_moi; mass01+=norm_xy_sq;
	mass10+=norm_xy_sq; mass11+=pivot->world_anch_b.x*pivot->world_anch_b.x*pivot->body_b->inv_moi;

	// set the matrix
	inv_determ=1.0/(mass00*mass11-mass01*mass10);
	pivot->mass0.x=mass11*inv_determ; pivot->mass0.y=-mass01*inv_determ;
	pivot->mass1.x=-mass10*inv_determ; pivot->mass1.y=mass00*inv_determ;

	delta=vector2_sub(vector2_add(pos_b,pivot->world_anch_b),vector2_add(pos_a,pivot->world_anch_a));
	pivot->bias=delta;
	pivot->bias_j.x=pivot->bias_j.y=0.0;
	
	rat_body_apply_offset_impulse(pivot->body_a,vector2_negative(pivot->norm_acc_j),pivot->world_anch_a);
	rat_body_apply_offset_impulse(pivot->body_b,pivot->norm_acc_j,pivot->world_anch_b);
}

void rat_constraint_pivot_step(rat_constraint *constraint)
{
	vector2 zerovec={0,0};
	rat_constraint_pivot *pivot=(rat_constraint_pivot *)constraint;
	vector2 bias_imp,vel_a,vel_b,vel_ab,impulse,bias_vel_a,bias_vel_b,bias_vel_ab;

	bias_vel_a=vector2_add(pivot->body_a->velocity_bias,
		vector2_multf(vector2_perp_left(pivot->world_anch_a),pivot->body_a->omega_bias));
	bias_vel_b=vector2_add(pivot->body_b->velocity_bias,
		vector2_multf(vector2_perp_left(pivot->world_anch_b),pivot->body_b->omega_bias));

	bias_vel_ab=vector2_add(vector2_sub(bias_vel_b,bias_vel_a),pivot->bias);
	
	bias_imp.x=-vector2_dot(bias_vel_ab,pivot->mass0);
	bias_imp.y=-vector2_dot(bias_vel_ab,pivot->mass1);
	pivot->bias_j=vector2_add(pivot->bias_j,bias_imp);
	
	rat_body_apply_offset_bias_impulse(pivot->body_a,vector2_negative(bias_imp),pivot->world_anch_a);
	rat_body_apply_offset_bias_impulse(pivot->body_b,bias_imp,pivot->world_anch_b);
	
	vel_a=vector2_add(pivot->body_a->velocity,
		vector2_multf(vector2_perp_left(pivot->world_anch_a),pivot->body_a->omega));
	vel_b=vector2_add(pivot->body_b->velocity,
		vector2_multf(vector2_perp_left(pivot->world_anch_b),pivot->body_b->omega));

	vel_ab=vector2_sub(vel_b,vel_a);
	
	impulse.x=-vector2_dot(vel_ab,pivot->mass0);
	impulse.y=-vector2_dot(vel_ab,pivot->mass1);
	pivot->norm_acc_j=vector2_add(pivot->norm_acc_j,impulse);

	rat_body_apply_offset_impulse(pivot->body_a,vector2_negative(impulse),pivot->world_anch_a);
	rat_body_apply_offset_impulse(pivot->body_b,impulse,pivot->world_anch_b);
}