Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Simplify angle derivative in apply_torsion_forces #14

Closed
wants to merge 1 commit into from
Closed

Simplify angle derivative in apply_torsion_forces #14

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 16 additions & 19 deletions packages/cadmium/src/sketch/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -734,9 +734,10 @@ impl Sketch {
let point_a = self.points.get(&point_a_id).unwrap();
let point_b = self.points.get(&point_b_id).unwrap();

let dt = 0.01;
let dx = point_b.x - point_a.x;
let dy = point_b.y - point_a.y;

let angle = (point_b.y - point_a.y).atan2(point_b.x - point_a.x);
let angle = dy.atan2(dx);

let mut err = rest - angle;
// println!("Err: {}", err);
Expand All @@ -747,25 +748,21 @@ impl Sketch {
err = err + PI * 2.0;
}

let point_a_stepped = point_a.step(dt);
let point_b_stepped = point_b.step(dt);
let angle_stepped = (point_b_stepped.1 - point_a_stepped.1)
.atan2(point_b_stepped.0 - point_a_stepped.0);
let mut angle_change = angle_stepped - angle;
// println!("Dangle: {}", angle_change);

if angle_change > PI {
angle_change = angle_change - PI * 2.0;
}
if angle_change < -PI {
angle_change = angle_change + PI * 2.0;
}

let d_angle = angle_change / dt;
// Atan2(y,x) = atan(y/x) + C with C \in {-pi, 0, pi} depending on the quadrant
// If y = y(t) and x = x(t) are the coordinates of the point at time t, then
// the derivative of atan2(y,x) with respect to time is
//
// 1 / ((y/x)^2 + 1) * (x * y' - y * x') / x^2,
// \---------------/ \---------------------/
// | |
// d(atan(u(t))) / du d(u(t)) / dt
//
// via the chain rule. Rewriting the denominator of the first term as (x^2 + y^2) / x^2
// and simplifying yields: (x * y' - y * x') / (x^2 + y^2).
let d_angle = (dx * (point_b.dy - point_a.dy) - dy * (point_b.dx - point_a.dx))
/ (dx * dx + dy * dy);
let torque = kp * err - kd * d_angle;

let dx = point_b.x - point_a.x;
let dy = point_b.y - point_a.y;
let dist = dx.hypot(dy);

let f_mag = torque / dist;
Expand Down
Loading