Repository: godotengine/godot
File: modules/godot_physics_2d/godot_space_2d.cpp (C++)

Code before:

			int amount = _cull_aabb_for_body(p_body, body_aabb);

			for (int j = 0; j < p_body->get_shape_count(); j++) {
				if (p_body->is_shape_disabled(j)) {
					continue;
				}

				GodotShape2D *body_shape = p_body->get_shape(j);
				Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);

				for (int i = 0; i < amount; i++) {
					const GodotCollisionObject2D *col_obj = intersection_query_results[i];
					if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
						continue;
					}
					if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
						continue;
					}

					int shape_idx = intersection_query_subindex_results[i];

					Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);

					if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
						cbk.valid_dir = col_obj->get_shape_one_way_collision_direction(shape_idx).rotated(col_obj_shape_xform.get_rotation());

						real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
						cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
						cbk.invalid_by_dir = 0;

						if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
							const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
							if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
								//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
								Vector2 lv = b->get_linear_velocity();
								//compute displacement from linear velocity
								Vector2 motion = lv * last_step;
								real_t motion_len = motion.length();
								motion.normalize();
								cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
							}
						}
					} else {
						cbk.valid_dir = Vector2();
						cbk.valid_depth = 0;
						cbk.invalid_by_dir = 0;
					}

					int current_passed = cbk.passed; //save how many points passed collision
					bool did_collide = false;

Review comment (suggestion):
I think [`atan2`](https://github.com/godotengine/godot/blob/780b6891e6f2a99e860a8c30eaf4c3dd6fdd13f7/core/math/transform_2d.cpp#L82-L84) followed by [`sin` and `cos`](https://github.com/godotengine/godot/blob/a4bbad2ba8a8ecd4e756e49de5c83666f12a9bd5/core/math/vector2.cpp#L112-L118) is not the best approach here.

Try [`Transform2D::basis_xform`](https://github.com/godotengine/godot/blob/a4bbad2ba8a8ecd4e756e49de5c83666f12a9bd5/core/math/transform_2d.h#L202-L206) followed by `Vector2::normalized` instead; that will just be a bunch of multiplications and additions and one square root.

(The same goes for all other occurrences of this pattern.)

Code after:

			int amount = _cull_aabb_for_body(p_body, body_aabb);

			for (int j = 0; j < p_body->get_shape_count(); j++) {
				if (p_body->is_shape_disabled(j)) {
					continue;
				}

				GodotShape2D *body_shape = p_body->get_shape(j);
				Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);

				for (int i = 0; i < amount; i++) {
					const GodotCollisionObject2D *col_obj = intersection_query_results[i];
					if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
						continue;
					}
					if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
						continue;
					}

					int shape_idx = intersection_query_subindex_results[i];

					Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);

					if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
						cbk.valid_dir = col_obj_shape_xform.basis_xform(col_obj->get_shape_one_way_collision_direction(shape_idx)).normalized();

						real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
						cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
						cbk.invalid_by_dir = 0;

						if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
							const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
							if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
								//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
								Vector2 lv = b->get_linear_velocity();
								//compute displacement from linear velocity
								Vector2 motion = lv * last_step;
								real_t motion_len = motion.length();
								motion.normalize();
								cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
							}
						}
					} else {
						cbk.valid_dir = Vector2();
						cbk.valid_depth = 0;
						cbk.invalid_by_dir = 0;
					}

					int current_passed = cbk.passed; //save how many points passed collision
					bool did_collide = false;