Commit 6e705ff1 authored by Libor Moravčík's avatar Libor Moravčík
Browse files

Fixed Slider Joint

parent 22714035

phx/include/phx/dyn/joint.hpp

+1 −0
Original line number Diff line number Diff line
@@ -160,6 +160,7 @@ struct SliderJoint : Joint 
	vec3 m_axis_a_ls;

	vec3 m_anchor_a_ls;

	vec3 m_anchor_b_ls;

	quat m_q0_a;



	// limits and motor

	std::optional<Limits> m_limits = std::nullopt;

phx/src/dyn/joint.cpp

+79 −32
Original line number Diff line number Diff line
@@ -149,7 +149,15 @@ void DistanceJoint::apply_impl(ConstraintsSolver &cs) 
	vec3 r1 = p1 - x1;

	vec3 r2 = p2 - x2;

	vec3 D = p2 - p1;

	vec3 n = normalized(D);

	vec3 n;



	if (equal(len2(D), 0.0f))

	{

		n = vec3{1, 0, 0};

	} else 

	{

	 	n = normalized(D);

	}



	scalar beta = 0.1f;

	scalar bias = (beta * (0.5f * len(D) - m_distance)) / phx::timer()->fixed_time_step();
@@ -172,8 +180,7 @@ void DistanceJoint::apply_impl(ConstraintsSolver &cs) 


SliderJoint::SliderJoint(std::string const &name, vec3 const &axis_ws, vec3 const &anchor_ws)

    : Joint(name, Type::SLIDER, 7), m_axis_ws(axis_ws), m_anchor_ws(anchor_ws)

{

}

{}



void SliderJoint::initialize()

{
@@ -183,6 +190,7 @@ void SliderJoint::initialize() 
	m_anchor_a_ls = rb_a->frame()->transform_position_in(m_anchor_ws);

	m_anchor_b_ls = rb_b->frame()->transform_position_in(m_anchor_ws);

	m_axis_a_ls = rb_a->frame()->transform_direction_in(m_axis_ws);

	m_q0_a = conjugate(rb_a->frame()->rotation()) * rb_b->frame()->rotation();

}



void SliderJoint::apply_impl(ConstraintsSolver &cs)
@@ -238,30 +246,74 @@ void SliderJoint::apply_impl(ConstraintsSolver &cs) 
			     lmaxfn, 

			     m_constraint_ids[1]);



	vec3 bias_rot = (beta * (theta_b - theta_a)) / (scalar) phx::timer()->fixed_time_step();

	bias_rot = -bias_rot;

	cs.insert_constraint(rb_a, 

			     rb_b, 

			     {zero<vec3>(), vec3(-1, 0, 0)},

			     {zero<vec3>(), vec3(1, 0, 0)}, 

			     bias_rot.x, 

			     lminfn, 

			     lmaxfn, m_constraint_ids[2]);

	// Rotational Constraints

	auto left_matrix = [](quat const &q)

	{

		mat4x4 m;

		m[0] = {q.w, -q.x, -q.y, -q.z};

		m[1] = {q.x,  q.w, -q.z,  q.y};

		m[2] = {q.y,  q.z,  q.w, -q.x};

		m[3] = {q.z, -q.y,  q.x,  q.w};

		return m;

	};



	cs.insert_constraint(rb_a, 

			     rb_b, 

			      {zero<vec3>(), vec3(0, -1, 0)},

				   {zero<vec3>(), vec3(0, 1, 0)}, 

				   bias_rot.y, 

				   lminfn, 

				   lmaxfn, m_constraint_ids[3]);

	cs.insert_constraint(rb_a, 

			     rb_b,

			     {zero<vec3>(), vec3(0, 0, -1)},

			     {zero<vec3>(), vec3(0, 0, 1)}, 

			     bias_rot.z, 

			     lminfn, 

			     lmaxfn, m_constraint_ids[4]);

	auto right_matrix = [](quat const &q)

	{

		mat4x4 m;

		m[0] = {q.w, -q.x, -q.y, -q.z};

		m[1] = {q.x,  q.w, q.z, -q.y};

		m[2] = {q.y, -q.z, q.w, q.x};

		m[3] = {q.z,  q.y, -q.x, q.w};

		return m;

	};



	auto q2vec = [](quat const &q) { return vec4(q.w, q.x, q.y, q.z); };



	mat4x4 P = {0, 0, 0, 0, 

		    0, 1, 0, 0, 

		    0, 0, 1, 0, 

		    0, 0, 0, 1};



	quat qa0conj = conjugate(m_q0_a);

	quat qa = rb_a->frame()->rotation();

	quat qb = rb_b->frame()->rotation();

	quat qaconj = conjugate(qa);

	quat qbconj = conjugate(qb);

	quat qr = qaconj * qb;



	vec3 x, y, z;

	x = rb_a->frame()->right();

	y = rb_a->frame()->forward();

	z = rb_a->frame()->up();



	beta = 0.1f;



	auto lminfn_rot = [](std::vector<scalar> const &)

	{ return -std::numeric_limits<scalar>::max(); };

	auto lmaxfn_rot = [](std::vector<scalar> const &)

	{ return std::numeric_limits<scalar>::max(); };



	vec3 bias = zero<vec3>();



	mat4x4 matA = P * left_matrix(qaconj) * right_matrix(qb * qa0conj) * P * -0.5f;

	mat4x4 matB = P * left_matrix(qaconj) * right_matrix(qb * qa0conj) * P * 0.5f;

	vec4   tmpA = matA * vec4(0, x.x, x.y, x.z);

	vec4   tmpB = matB * vec4(0, x.x, x.y, x.z);

	vec3   compA = vec3(tmpA[1], tmpA[2], tmpA[3]);

	vec3   compB = vec3(tmpB[1], tmpB[2], tmpB[3]);

	m_constraint_ids[3] = cs.insert_constraint(rb_a, rb_b, {zero<vec3>(), compA}, {zero<vec3>(), compB}, bias.x, lminfn_rot, lmaxfn_rot, m_constraint_ids[3]);



	tmpA = matA * vec4(0, y.x, y.y, y.z);

	tmpB = matB * vec4(0, y.x, y.y, y.z);

	compA = vec3(tmpA[1], tmpA[2], tmpA[3]);

	compB = vec3(tmpB[1], tmpB[2], tmpB[3]);

	m_constraint_ids[4] = cs.insert_constraint(rb_a, rb_b, {zero<vec3>(), compA}, {zero<vec3>(), compB}, bias.y, lminfn_rot, lmaxfn_rot, m_constraint_ids[4]);



	tmpA = matA * vec4(0, z.x, z.y, z.z);

	tmpB = matB * vec4(0, z.x, z.y, z.z);

	compA = vec3(tmpA[1], tmpA[2], tmpA[3]);

	compB = vec3(tmpB[1], tmpB[2], tmpB[3]);

	m_constraint_ids[5] = cs.insert_constraint(rb_a, rb_b, {zero<vec3>(), compA}, {zero<vec3>(), compB}, bias.z, lminfn_rot, lmaxfn_rot, m_constraint_ids[5]);



	auto lminfn_dist = [](std::vector<scalar> const &)

	{ return 0.0f; };
@@ -410,11 +462,6 @@ void FixedJoint::apply_impl(ConstraintsSolver &cs) 
	auto lmaxfn_rot = [](std::vector<scalar> const &)

	{ return std::numeric_limits<scalar>::max(); };



	// vec4 tmp = P * right_matrix(qa0conj) * q2vec(qr);

	// C  = vec3(tmp[1], tmp[2], tmp[3]);

	// bias = vec3(dot(C, x), dot(C,y), dot(C, z));

	// bias = (beta * C) / (scalar) phx::timer()->fixed_time_step();

	// bias = -bias;

	bias = zero<vec3>();



	mat4x4 matA = P * left_matrix(qaconj) * right_matrix(qb * qa0conj) * P * -0.5f;