Skip to content

Commit

Permalink
Don't care on cases.
Browse files Browse the repository at this point in the history
  • Loading branch information
@fire
fire committed Dec 9, 2023
1 parent c3a1ae1 commit 7e2b0d1
Showing 1 changed file with 63 additions and 41 deletions.
104 changes: 63 additions & 41 deletions src/ik_limit_cone_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,10 +196,10 @@ bool IKLimitCone3D::_determine_if_in_bounds(Ref<IKLimitCone3D> next, Vector3 inp

if (control_point.dot(input) >= radius_cosine) {
return true;
} else if (next != nullptr && next->control_point.dot(input) >= next->radius_cosine) {
} else if (next.is_valid() && next->control_point.dot(input) >= next->radius_cosine) {
return true;
} else {
if (next == nullptr) {
if (next.is_null()) {
return false;
}
bool inTan1Rad = tangent_circle_center_next_1.dot(input) > tangent_circle_radius_next_cos;
Expand Down Expand Up @@ -239,21 +239,31 @@ bool IKLimitCone3D::_determine_if_in_bounds(Ref<IKLimitCone3D> next, Vector3 inp
}

Vector3 IKLimitCone3D::get_closest_path_point(Ref<IKLimitCone3D> next, Vector3 input) const {
Vector3 result = _get_on_path_sequence(next, input);
bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z));
if (!is_number) {
result = _closest_cone(next, input);
Vector3 result;
if (next.is_null()) {
result = _closest_cone(Ref<IKLimitCone3D>(this), input);
} else {
result = _get_on_path_sequence(next, input);
bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z));
if (!is_number) {
result = _closest_cone(next, input);
}
}
return result;
}

Vector3 IKLimitCone3D::_get_closest_collision(Ref<IKLimitCone3D> next, Vector3 input) const {
Vector3 result = get_on_great_tangent_triangle(next, input);

bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z));
if (!is_number) {
Vector3 result;
if (next.is_null()) {
Vector<double> in_bounds = { 0.0 };
result = _closest_point_on_closest_cone(next, input, &in_bounds);
result = _closest_cone(Ref<IKLimitCone3D>(), input);
} else {
result = get_on_great_tangent_triangle(next, input);
bool is_number = !(Math::is_nan(result.x) && Math::is_nan(result.y) && Math::is_nan(result.z));
if (!is_number) {
Vector<double> in_bounds = { 0.0 };
result = _closest_point_on_closest_cone(next, input, &in_bounds);
}
}
return result;
}
Expand Down Expand Up @@ -322,10 +332,14 @@ Vector3 IKLimitCone3D::get_on_great_tangent_triangle(Ref<IKLimitCone3D> next, Ve
}

Vector3 IKLimitCone3D::_closest_cone(Ref<IKLimitCone3D> next, Vector3 input) const {
if (input.dot(control_point) > input.dot(next->control_point)) {
if (next.is_null()) {
return control_point;
} else {
return next->control_point;
if (input.dot(control_point) > input.dot(next->control_point)) {
return control_point;
} else {
return next->control_point;
}
}
}

Expand All @@ -334,17 +348,21 @@ Vector3 IKLimitCone3D::_closest_point_on_closest_cone(Ref<IKLimitCone3D> next, V
if ((*in_bounds)[0] > 0.0) {
return closestToFirst;
}
Vector3 closestToSecond = next->closest_to_cone(input, in_bounds);
if ((*in_bounds)[0] > 0.0) {
return closestToSecond;
}
double cosToFirst = input.dot(closestToFirst);
double cosToSecond = input.dot(closestToSecond);

if (cosToFirst > cosToSecond) {
if (next.is_null()) {
return closestToFirst;
} else {
return closestToSecond;
Vector3 closestToSecond = next->closest_to_cone(input, in_bounds);
if ((*in_bounds)[0] > 0.0) {
return closestToSecond;
}
double cosToFirst = input.dot(closestToFirst);
double cosToSecond = input.dot(closestToSecond);

if (cosToFirst > cosToSecond) {
return closestToFirst;
} else {
return closestToSecond;
}
}
}

Expand Down Expand Up @@ -378,27 +396,31 @@ void IKLimitCone3D::set_tangent_circle_center_next_2(Vector3 point) {
}

Vector3 IKLimitCone3D::_get_on_path_sequence(Ref<IKLimitCone3D> next, Vector3 input) const {
Vector3 c1xc2 = get_control_point().cross(next->control_point).normalized();
double c1c2dir = input.dot(c1xc2);
if (c1c2dir < 0.0) {
Vector3 c1xt1 = get_control_point().cross(tangent_circle_center_next_1).normalized();
Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->get_control_point()).normalized();
if (input.dot(c1xt1) > 0.0f && input.dot(t1xc2) > 0.0f) {
Ref<IKRay3D> tan1ToInput = Ref<IKRay3D>(memnew(IKRay3D(tangent_circle_center_next_1, input)));
Vector3 result = tan1ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point());
return result.normalized();
} else {
return Vector3(NAN, NAN, NAN);
}
if (next.is_null()) {
return Vector3(NAN, NAN, NAN);
} else {
Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point).normalized();
Vector3 c2xt2 = next->get_control_point().cross(tangent_circle_center_next_2).normalized();
if (input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0) {
Ref<IKRay3D> tan2ToInput = Ref<IKRay3D>(memnew(IKRay3D(tangent_circle_center_next_2, input)));
Vector3 result = tan2ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point());
return result.normalized();
Vector3 c1xc2 = get_control_point().cross(next->control_point).normalized();
double c1c2dir = input.dot(c1xc2);
if (c1c2dir < 0.0) {
Vector3 c1xt1 = get_control_point().cross(tangent_circle_center_next_1).normalized();
Vector3 t1xc2 = tangent_circle_center_next_1.cross(next->get_control_point()).normalized();
if (input.dot(c1xt1) > 0.0f && input.dot(t1xc2) > 0.0f) {
Ref<IKRay3D> tan1ToInput = Ref<IKRay3D>(memnew(IKRay3D(tangent_circle_center_next_1, input)));
Vector3 result = tan1ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point());
return result.normalized();
} else {
return Vector3(NAN, NAN, NAN);
}
} else {
return Vector3(NAN, NAN, NAN);
Vector3 t2xc1 = tangent_circle_center_next_2.cross(control_point).normalized();
Vector3 c2xt2 = next->get_control_point().cross(tangent_circle_center_next_2).normalized();
if (input.dot(t2xc1) > 0 && input.dot(c2xt2) > 0) {
Ref<IKRay3D> tan2ToInput = Ref<IKRay3D>(memnew(IKRay3D(tangent_circle_center_next_2, input)));
Vector3 result = tan2ToInput->get_intersects_plane(Vector3(0.0f, 0.0f, 0.0f), get_control_point(), next->get_control_point());
return result.normalized();
} else {
return Vector3(NAN, NAN, NAN);
}
}
}
}

0 comments on commit 7e2b0d1

Please sign in to comment.