Vector2 Subtract( Vector2 c_lhs, Vector2 const& i_rhs )
{
c_lhs.Subtract( i_rhs );
return c_lhs;
}
/**
* Gets the absolute scale of the object in local space
* @param result Storage for the position
*/
void Transform::GetLocalScale (Vector2& result)
{
result.setX(localTransform.mat[0][0]);
result.setY(localTransform.mat[1][1]);
}
/**
* Gets the absolute scale of the object in world space
* @param result Storage for the position
*/
void Transform::GetScale (Vector2& result)
{
result.setX(finalTransform.mat[0][0]);
result.setY(finalTransform.mat[1][1]);
}
Real DistRay2Ray2<Real>::GetSquared ()
{
Vector2<Real> diff = mRay0->Origin - mRay1->Origin;
Real a01 = -mRay0->Direction.Dot(mRay1->Direction);
Real b0 = diff.Dot(mRay0->Direction);
Real c = diff.SquaredLength();
Real det = Math<Real>::FAbs((Real)1 - a01*a01);
Real b1, s0, s1, sqrDist;
if (det >= Math<Real>::ZERO_TOLERANCE)
{
// Rays are not parallel.
b1 = -diff.Dot(mRay1->Direction);
s0 = a01*b1 - b0;
s1 = a01*b0 - b1;
if (s0 >= (Real)0)
{
if (s1 >= (Real)0) // region 0 (interior)
{
// Minimum at two interior points of rays.
Real invDet = ((Real)1)/det;
s0 *= invDet;
s1 *= invDet;
sqrDist = (Real)0;
}
else // region 3 (side)
{
s1 = (Real)0;
if (b0 >= (Real)0)
{
s0 = (Real)0;
sqrDist = c;
}
else
{
s0 = -b0;
sqrDist = b0*s0 + c;
}
}
}
else
{
if (s1 >= (Real)0) // region 1 (side)
{
s0 = (Real)0;
if (b1 >= (Real)0)
{
s1 = (Real)0;
sqrDist = c;
}
else
{
s1 = -b1;
sqrDist = b1*s1 + c;
}
}
else // region 2 (corner)
{
if (b0 < (Real)0)
{
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
}
else
{
s0 = (Real)0;
if (b1 >= (Real)0)
{
s1 = (Real)0;
sqrDist = c;
}
else
{
s1 = -b1;
sqrDist = b1*s1 + c;
}
}
}
}
}
else
{
// Rays are parallel.
if (a01 > (Real)0.0)
{
// Opposite direction vectors.
s1 = (Real)0;
if (b0 >= (Real)0)
{
s0 = (Real)0;
sqrDist = c;
}
else
{
s0 = -b0;
sqrDist = b0*s0 + c;
}
}
else
{
// Same direction vectors.
if (b0 >= (Real)0)
{
b1 = -diff.Dot(mRay1->Direction);
s0 = (Real)0;
s1 = -b1;
sqrDist = b1*s1 + c;
}
else
{
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
}
}
}
mClosestPoint0 = mRay0->Origin + s0*mRay0->Direction;
mClosestPoint1 = mRay1->Origin + s1*mRay1->Direction;
// Account for numerical round-off errors.
if (sqrDist < (Real)0)
{
sqrDist = (Real)0;
}
return sqrDist;
}
void Flower::drawPedals(PNG* canvas, const Vector2& center, int x, int y) const
{
Shape *pedal;
pedal = new Circle (center, PEDAL_COLOR, PEDAL_RADIUS);
pedal->set_center(Vector2(center.x()+x, center.y()+y));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()+x, center.y()-y));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()-x, center.y()+y));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()-x, center.y()-y));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()+y, center.y()+x));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()+y, center.y()-x));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()-y, center.y()+x));
pedal->draw(canvas);
pedal->set_center(Vector2(center.x()-y, center.y()-x));
pedal->draw(canvas);
delete pedal;
}
void JSONValue::AddVector2(const Vector2& value)
{
AddString(value.ToString());
}
bool Path2DEditor::forward_input_event(const InputEvent& p_event) {
if (!node)
return false;
if (!node->is_visible())
return false;
if (!node->get_curve().is_valid())
return false;
switch(p_event.type) {
case InputEvent::MOUSE_BUTTON: {
const InputEventMouseButton &mb=p_event.mouse_button;
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = Point2(mb.x,mb.y);
Vector2 cpoint = !mb.mod.alt? canvas_item_editor->snap_point(xform.affine_inverse().xform(gpoint))
: node->get_global_transform().affine_inverse().xform( canvas_item_editor->snap_point(canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint)) );
//first check if a point is to be added (segment split)
real_t grab_treshold=EDITOR_DEF("poly_editor/point_grab_radius",8);
// Test move point!!
if ( mb.pressed && action==ACTION_NONE ) {
Ref<Curve2D> curve = node->get_curve();
for(int i=0;i<curve->get_point_count();i++) {
bool pointunder=false;
{
Point2 p = xform.xform( curve->get_point_pos(i) );
if (gpoint.distance_to(p) < grab_treshold ) {
if (mb.button_index==BUTTON_LEFT && !mb.mod.shift && mode==MODE_EDIT) {
action=ACTION_MOVING_POINT;
action_point=i;
moving_from=curve->get_point_pos(i);
moving_screen_from=gpoint;
return true;
} else if ((mb.button_index==BUTTON_RIGHT && mode==MODE_EDIT) || (mb.button_index==BUTTON_LEFT && mode==MODE_DELETE)) {
undo_redo->create_action(TTR("Remove Point from Curve"));
undo_redo->add_do_method(curve.ptr(),"remove_point",i);
undo_redo->add_undo_method(curve.ptr(),"add_point",curve->get_point_pos(i),curve->get_point_in(i),curve->get_point_out(i),i);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
return true;
} else
pointunder=true;
}
}
if (mb.button_index==BUTTON_LEFT && i<(curve->get_point_count()-1)) {
Point2 p = xform.xform( curve->get_point_pos(i)+curve->get_point_out(i) );
if (gpoint.distance_to(p) < grab_treshold && (mode == MODE_EDIT || mode==MODE_EDIT_CURVE) ) {
action=ACTION_MOVING_OUT;
action_point=i;
moving_from=curve->get_point_out(i);
moving_screen_from=gpoint;
return true;
}
}
if (mb.button_index==BUTTON_LEFT && i>0) {
Point2 p = xform.xform( curve->get_point_pos(i)+curve->get_point_in(i) );
if (gpoint.distance_to(p) < grab_treshold && (mode == MODE_EDIT || mode==MODE_EDIT_CURVE)) {
action=ACTION_MOVING_IN;
action_point=i;
moving_from=curve->get_point_in(i);
moving_screen_from=gpoint;
return true;
}
}
if (pointunder)
return true;
}
}
// Test add point in empty space!
if ( mb.pressed && mb.button_index==BUTTON_LEFT && ((mb.mod.command && mode == MODE_EDIT) || mode == MODE_CREATE)) {
Ref<Curve2D> curve = node->get_curve();
undo_redo->create_action(TTR("Add Point to Curve"));
undo_redo->add_do_method(curve.ptr(),"add_point",cpoint);
undo_redo->add_undo_method(curve.ptr(),"remove_point",curve->get_point_count());
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
action=ACTION_MOVING_POINT;
action_point=curve->get_point_count()-1;
moving_from=curve->get_point_pos(action_point);
moving_screen_from=gpoint;
canvas_item_editor->get_viewport_control()->update();
return true;
}
if ( !mb.pressed && mb.button_index==BUTTON_LEFT && action!=ACTION_NONE) {
Ref<Curve2D> curve = node->get_curve();
Vector2 new_pos = moving_from + xform.affine_inverse().basis_xform(gpoint - moving_screen_from);
switch(action) {
case ACTION_MOVING_POINT: {
undo_redo->create_action(TTR("Move Point in Curve"));
undo_redo->add_do_method(curve.ptr(),"set_point_pos",action_point,cpoint);
undo_redo->add_undo_method(curve.ptr(),"set_point_pos",action_point,moving_from);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
} break;
case ACTION_MOVING_IN: {
undo_redo->create_action(TTR("Move In-Control in Curve"));
undo_redo->add_do_method(curve.ptr(),"set_point_in",action_point,new_pos);
undo_redo->add_undo_method(curve.ptr(),"set_point_in",action_point,moving_from);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
} break;
case ACTION_MOVING_OUT: {
undo_redo->create_action(TTR("Move Out-Control in Curve"));
undo_redo->add_do_method(curve.ptr(),"set_point_out",action_point,new_pos);
undo_redo->add_undo_method(curve.ptr(),"set_point_out",action_point,moving_from);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
} break;
}
action=ACTION_NONE;
return true;
}
#if 0
switch(mode) {
case MODE_CREATE: {
if (mb.button_index==BUTTON_LEFT && mb.pressed) {
if (!wip_active) {
wip.clear();
wip.push_back( canvas_item_editor->snap_point(cpoint) );
wip_active=true;
edited_point_pos=canvas_item_editor->snap_point(cpoint);
canvas_item_editor->update();
edited_point=1;
return true;
} else {
if (wip.size()>1 && xform.xform(wip[0]).distance_to(gpoint)<grab_treshold) {
//wip closed
_wip_close();
return true;
} else {
wip.push_back( canvas_item_editor->snap_point(cpoint) );
edited_point=wip.size();
canvas_item_editor->update();
return true;
//add wip point
}
}
} else if (mb.button_index==BUTTON_RIGHT && mb.pressed && wip_active) {
_wip_close();
}
} break;
case MODE_EDIT: {
if (mb.button_index==BUTTON_LEFT) {
if (mb.pressed) {
if (mb.mod.control) {
if (poly.size() < 3) {
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_undo_method(node,"set_polygon",poly);
poly.push_back(cpoint);
undo_redo->add_do_method(node,"set_polygon",poly);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
return true;
}
//search edges
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int i=0;i<poly.size();i++) {
Vector2 points[2] ={ xform.xform(poly[i]),
xform.xform(poly[(i+1)%poly.size()]) };
Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint,points);
if (cp.distance_squared_to(points[0])<CMP_EPSILON2 || cp.distance_squared_to(points[1])<CMP_EPSILON2)
continue; //not valid to reuse point
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_idx=i;
}
}
if (closest_idx>=0) {
pre_move_edit=poly;
poly.insert(closest_idx+1,canvas_item_editor->snap_point(xform.affine_inverse().xform(closest_pos)));
edited_point=closest_idx+1;
edited_point_pos=canvas_item_editor->snap_point(xform.affine_inverse().xform(closest_pos));
node->set_polygon(poly);
canvas_item_editor->update();
return true;
}
} else {
//look for points to move
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int i=0;i<poly.size();i++) {
Vector2 cp =xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_idx=i;
}
}
if (closest_idx>=0) {
pre_move_edit=poly;
edited_point=closest_idx;
edited_point_pos=xform.affine_inverse().xform(closest_pos);
canvas_item_editor->update();
return true;
}
}
} else {
if (edited_point!=-1) {
//apply
ERR_FAIL_INDEX_V(edited_point,poly.size(),false);
poly[edited_point]=edited_point_pos;
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_do_method(node,"set_polygon",poly);
undo_redo->add_undo_method(node,"set_polygon",pre_move_edit);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
edited_point=-1;
return true;
}
}
} if (mb.button_index==BUTTON_RIGHT && mb.pressed && edited_point==-1) {
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int i=0;i<poly.size();i++) {
Vector2 cp =xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_idx=i;
}
}
if (closest_idx>=0) {
undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
undo_redo->add_undo_method(node,"set_polygon",poly);
poly.remove(closest_idx);
undo_redo->add_do_method(node,"set_polygon",poly);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
return true;
}
}
} break;
}
#endif
} break;
case InputEvent::MOUSE_MOTION: {
const InputEventMouseMotion &mm=p_event.mouse_motion;
if ( action!=ACTION_NONE) {
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = Point2(mm.x,mm.y);
Vector2 cpoint = !mm.mod.alt? canvas_item_editor->snap_point(xform.affine_inverse().xform(gpoint))
: node->get_global_transform().affine_inverse().xform( canvas_item_editor->snap_point(canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint)) );
Ref<Curve2D> curve = node->get_curve();
Vector2 new_pos = moving_from + xform.affine_inverse().basis_xform(gpoint - moving_screen_from);
switch(action) {
case ACTION_MOVING_POINT: {
curve->set_point_pos(action_point,cpoint);
} break;
case ACTION_MOVING_IN: {
curve->set_point_in(action_point,new_pos);
} break;
case ACTION_MOVING_OUT: {
curve->set_point_out(action_point,new_pos);
} break;
}
canvas_item_editor->get_viewport_control()->update();
return true;
}
#if 0
if (edited_point!=-1 && (wip_active || mm.button_mask&BUTTON_MASK_LEFT)) {
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = Point2(mm.x,mm.y);
edited_point_pos = canvas_item_editor->snap_point(xform.affine_inverse().xform(gpoint));
canvas_item_editor->update();
}
#endif
} break;
}
return false;
}
void Polygon2DEditor::_uv_input(const InputEvent& p_input) {
Matrix32 mtx;
mtx.elements[2]=-uv_draw_ofs;
mtx.scale_basis(Vector2(uv_draw_zoom,uv_draw_zoom));
if (p_input.type==InputEvent::MOUSE_BUTTON) {
const InputEventMouseButton &mb=p_input.mouse_button;
if (mb.button_index==BUTTON_LEFT) {
if (mb.pressed) {
uv_drag_from=Vector2(mb.x,mb.y);
uv_drag=true;
uv_prev=node->get_uv();
uv_move_current=uv_mode;
if (uv_move_current==UV_MODE_EDIT_POINT) {
if (mb.mod.shift && mb.mod.command)
uv_move_current=UV_MODE_SCALE;
else if (mb.mod.shift)
uv_move_current=UV_MODE_MOVE;
else if (mb.mod.command)
uv_move_current=UV_MODE_ROTATE;
}
if (uv_move_current==UV_MODE_EDIT_POINT) {
uv_drag_index=-1;
for(int i=0; i<uv_prev.size(); i++) {
Vector2 tuv=mtx.xform(uv_prev[i]);
if (tuv.distance_to(Vector2(mb.x,mb.y))<8) {
uv_drag_index=i;
}
}
if (uv_drag_index==-1) {
uv_drag=false;
}
}
} else if (uv_drag) {
undo_redo->create_action("Transform UV Map");
undo_redo->add_do_method(node,"set_uv",node->get_uv());
undo_redo->add_undo_method(node,"set_uv",uv_prev);
undo_redo->add_do_method(uv_edit_draw,"update");
undo_redo->add_undo_method(uv_edit_draw,"update");
undo_redo->commit_action();
uv_drag=false;
}
} else if (mb.button_index==BUTTON_RIGHT && mb.pressed) {
if (uv_drag) {
uv_drag=false;
node->set_uv(uv_prev);
uv_edit_draw->update();
}
} else if (mb.button_index==BUTTON_WHEEL_UP && mb.pressed) {
uv_zoom->set_val( uv_zoom->get_val()/0.9 );
} else if (mb.button_index==BUTTON_WHEEL_DOWN && mb.pressed) {
uv_zoom->set_val( uv_zoom->get_val()*0.9);
}
} else if (p_input.type==InputEvent::MOUSE_MOTION) {
const InputEventMouseMotion &mm=p_input.mouse_motion;
if (mm.button_mask&BUTTON_MASK_MIDDLE || Input::get_singleton()->is_key_pressed(KEY_SPACE)) {
Vector2 drag(mm.relative_x,mm.relative_y);
uv_hscroll->set_val( uv_hscroll->get_val()-drag.x );
uv_vscroll->set_val( uv_vscroll->get_val()-drag.y );
} else if (uv_drag) {
Vector2 uv_drag_to(mm.x,mm.y);
Vector2 drag = mtx.affine_inverse().xform(uv_drag_to) - mtx.affine_inverse().xform(uv_drag_from);
switch(uv_move_current) {
case UV_MODE_EDIT_POINT: {
DVector<Vector2> uv_new=uv_prev;
uv_new.set( uv_drag_index, uv_new[uv_drag_index]+drag );
node->set_uv(uv_new);
}
break;
case UV_MODE_MOVE: {
DVector<Vector2> uv_new=uv_prev;
for(int i=0; i<uv_new.size(); i++)
uv_new.set( i, uv_new[i]+drag );
node->set_uv(uv_new);
}
break;
case UV_MODE_ROTATE: {
Vector2 center;
DVector<Vector2> uv_new=uv_prev;
for(int i=0; i<uv_new.size(); i++)
center+=uv_prev[i];
center/=uv_new.size();
float angle = (uv_drag_from - mtx.xform(center)).normalized().angle_to( (uv_drag_to - mtx.xform(center)).normalized() );
for(int i=0; i<uv_new.size(); i++) {
Vector2 rel = uv_prev[i] - center;
rel=rel.rotated(angle);
uv_new.set(i,center+rel);
}
node->set_uv(uv_new);
}
break;
case UV_MODE_SCALE: {
Vector2 center;
DVector<Vector2> uv_new=uv_prev;
for(int i=0; i<uv_new.size(); i++)
center+=uv_prev[i];
center/=uv_new.size();
float from_dist = uv_drag_from.distance_to(mtx.xform(center));
float to_dist = uv_drag_to.distance_to(mtx.xform(center));
if (from_dist<2)
break;
float scale = to_dist/from_dist;
for(int i=0; i<uv_new.size(); i++) {
Vector2 rel = uv_prev[i] - center;
rel=rel*scale;
uv_new.set(i,center+rel);
}
node->set_uv(uv_new);
}
break;
}
uv_edit_draw->update();
}
}
}
bool NavigationPolygonEditor::forward_input_event(const InputEvent& p_event) {
if (!node)
return false;
if (node->get_navigation_polygon().is_null()) {
if (p_event.type==InputEvent::MOUSE_BUTTON && p_event.mouse_button.button_index==1 && p_event.mouse_button.pressed) {
create_nav->set_text("No NavigationPolygon resource on this node.\nCreate and assign one?");
create_nav->popup_centered_minsize();
}
return (p_event.type==InputEvent::MOUSE_BUTTON && p_event.mouse_button.button_index==1);;
}
switch(p_event.type) {
case InputEvent::MOUSE_BUTTON: {
const InputEventMouseButton &mb=p_event.mouse_button;
Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = Point2(mb.x,mb.y);
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint=canvas_item_editor->snap_point(cpoint);
cpoint = node->get_global_transform().affine_inverse().xform(cpoint);
//first check if a point is to be added (segment split)
real_t grab_treshold=EDITOR_DEF("poly_editor/point_grab_radius",8);
switch(mode) {
case MODE_CREATE: {
if (mb.button_index==BUTTON_LEFT && mb.pressed) {
if (!wip_active) {
wip.clear();
wip.push_back( cpoint );
wip_active=true;
edited_point_pos=cpoint;
edited_outline=-1;
canvas_item_editor->get_viewport_control()->update();
edited_point=1;
return true;
} else {
if (wip.size()>1 && xform.xform(wip[0]).distance_to(gpoint)<grab_treshold) {
//wip closed
_wip_close();
return true;
} else {
wip.push_back( cpoint );
edited_point=wip.size();
canvas_item_editor->get_viewport_control()->update();
return true;
//add wip point
}
}
} else if (mb.button_index==BUTTON_RIGHT && mb.pressed && wip_active) {
_wip_close();
}
} break;
case MODE_EDIT: {
if (mb.button_index==BUTTON_LEFT) {
if (mb.pressed) {
if (mb.mod.control) {
//search edges
int closest_outline=-1;
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int j=0;j<node->get_navigation_polygon()->get_outline_count();j++) {
DVector<Vector2> points=node->get_navigation_polygon()->get_outline(j);
int pc=points.size();
DVector<Vector2>::Read poly=points.read();
for(int i=0;i<pc;i++) {
Vector2 points[2] ={ xform.xform(poly[i]),
xform.xform(poly[(i+1)%pc]) };
Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint,points);
if (cp.distance_squared_to(points[0])<CMP_EPSILON2 || cp.distance_squared_to(points[1])<CMP_EPSILON2)
continue; //not valid to reuse point
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_outline=j;
closest_pos=cp;
closest_idx=i;
}
}
}
if (closest_idx>=0) {
pre_move_edit=node->get_navigation_polygon()->get_outline(closest_outline);
DVector<Point2> poly = pre_move_edit;
poly.insert(closest_idx+1,xform.affine_inverse().xform(closest_pos));
edited_point=closest_idx+1;
edited_outline=closest_outline;
edited_point_pos=xform.affine_inverse().xform(closest_pos);
node->get_navigation_polygon()->set_outline(closest_outline,poly);
canvas_item_editor->get_viewport_control()->update();
return true;
}
} else {
//look for points to move
int closest_outline=-1;
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int j=0;j<node->get_navigation_polygon()->get_outline_count();j++) {
DVector<Vector2> points=node->get_navigation_polygon()->get_outline(j);
int pc=points.size();
DVector<Vector2>::Read poly=points.read();
for(int i=0;i<pc;i++) {
Vector2 cp =xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_outline=j;
closest_idx=i;
}
}
}
if (closest_idx>=0) {
pre_move_edit=node->get_navigation_polygon()->get_outline(closest_outline);
edited_point=closest_idx;
edited_outline=closest_outline;
edited_point_pos=xform.affine_inverse().xform(closest_pos);
canvas_item_editor->get_viewport_control()->update();
return true;
}
}
} else {
if (edited_point!=-1) {
//apply
DVector<Vector2> poly = node->get_navigation_polygon()->get_outline(edited_outline);
ERR_FAIL_INDEX_V(edited_point,poly.size(),false);
poly.set(edited_point,edited_point_pos);
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"set_outline",edited_outline,poly);
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"set_outline",edited_outline,pre_move_edit);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
edited_point=-1;
return true;
}
}
} if (mb.button_index==BUTTON_RIGHT && mb.pressed && edited_point==-1) {
int closest_outline=-1;
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int j=0;j<node->get_navigation_polygon()->get_outline_count();j++) {
DVector<Vector2> points=node->get_navigation_polygon()->get_outline(j);
int pc=points.size();
DVector<Vector2>::Read poly=points.read();
for(int i=0;i<pc;i++) {
Vector2 cp =xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_outline=j;
closest_idx=i;
}
}
}
if (closest_idx>=0) {
DVector<Vector2> poly = node->get_navigation_polygon()->get_outline(closest_outline);
if (poly.size()>3) {
undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"set_outline",closest_outline,poly);
poly.remove(closest_idx);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"set_outline",closest_outline,poly);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
} else {
undo_redo->create_action(TTR("Remove Poly And Point"));
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"add_outline_at_index",poly,closest_outline);
poly.remove(closest_idx);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"remove_outline",closest_outline);
undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
}
return true;
}
}
} break;
}
} break;
case InputEvent::MOUSE_MOTION: {
const InputEventMouseMotion &mm=p_event.mouse_motion;
if (edited_point!=-1 && (wip_active || mm.button_mask&BUTTON_MASK_LEFT)) {
Vector2 gpoint = Point2(mm.x,mm.y);
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint=canvas_item_editor->snap_point(cpoint);
edited_point_pos = node->get_global_transform().affine_inverse().xform(cpoint);
canvas_item_editor->get_viewport_control()->update();
}
} break;
}
return false;
}
Vector2 Normalize( Vector2 c_vector )
{
c_vector.Normalize();
return c_vector;
}
float Obstacle::circleIntersection(const Vector2& dir, const Vector2& start, float radius) const {
const float radSqd = radius * radius;
const float SPEED = abs(dir);
Vector2 forward(dir / SPEED);
// Find the end points relative to the start position
Vector2 a = getP0() - start;
Vector2 b = getP1() - start;
// rotate the segment so that the direction is aligned with the x-axis
// TODO: Where is this exploited???
float x = a.x() * forward.x() + a.y() * forward.y();
float y = a.y() * forward.x() - a.x() * forward.y();
a.set(x, y);
x = b.x() * forward.x() + b.y() * forward.y();
y = b.y() * forward.x() - b.x() * forward.y();
b.set(x, y);
// compute the implicit equation of the obstacle line
Vector2 disp = b - a;
float dist = abs(disp);
Vector2 D = disp / dist;
Vector2 N(D.y(), -D.x());
float C = -(N * a); // Ax + By + C = 0 --> implicit equation
// Test for collision
if (C < 0.f) {
// the agent lies on the "wrong" side of the obstacle and can't see it.
return INFTY;
} else if (C < radius) { // the circle overlaps the line on the visible side
float t = D * (-a); // projection of origin on the line
if (t >= -radius && t <= dist + radius) {
// The projection of the circle center lies within the projection of
// the minkowski sum on the line (i.e. extends past the points by
// a distance equal to the radius).
if ((t >= 0 && t <= dist) || (t < 0 && absSq(a) < radSqd) ||
(t > dist && absSq(b) < radSqd)) {
return 0.f;
}
}
}
// Not currently colliding -- now compute potential collision in the future
// M points to the side of the line on which the origin (aka agent) lies
// This creates the leading edge of the minkowski sum (defined by (a2, b2)).
Vector2 M(C < 0.f ? -N : N);
Vector2 a2(a + M * radius);
Vector2 b2(b + M * radius);
// I use this to do quick and dirty floating-point SIGN tests
// This may not be particularly portable
union {
float f;
unsigned int u;
} w1, w2;
w1.f = a2.y();
w2.f = b2.y();
if ((w1.u ^ w2.u) & 0x80000000) {
// signs of the y-values are different; the segment crosses the line
float t = -a2.y() / D.y();
float x = a2.x() + D.x() * t;
if (x > 0) {
// The time it takes to travel distance x
return x / SPEED;
}
} else {
// both end points are on the same side of the line
// Note: Both of these are possible if the obstacle is near parallel
// to the forward direction
float minT = INFTY;
float aDist2 = a.y() * a.y();
if (aDist2 < radSqd) {
// collision with a
// dx < radius
float dx = sqrtf(radSqd - aDist2);
float x = a.x() - dx; // collision point candidate
// This is a bit tricky - I don't have to consider a.x() + dx
// 1) the direction is in the positive x-axis direction, so I know
// the earliest collision must have a lesser x-value.
// 2) It's POSSIBLE for x to have a negative value, but if that's
// true, then a.x() + dx must ALSO be negative, otherwise
// the point is inside the circle and it would be detected
// as a collision. So, it's enough to just test one value
if (x > 0.f) {
float t = x / (dist * D.x());
if (t < minT) {
minT = t;
}
}
}
float bDist2 = b.y() * b.y();
if (bDist2 < radSqd) {
// collision with a
// dx < radius
float dx = sqrtf(radSqd - bDist2);
float x = b.x() - dx; // collision point candidate
if (x > 0.f) {
float t = x / dir.x();
if (t < minT) {
minT = t;
}
}
}
return minT;
}
return INFTY;
}
double DotProduct( Vector2 const& i_lhs, Vector2 const& i_rhs )
{
return i_lhs.DotProduct( i_rhs );
}
double Magnitude( Vector2 const& i_vector )
{
return i_vector.Magnitude();
}
Vector2 Scale( Vector2 c_lhs, double const& i_rhs )
{
c_lhs.Scale( i_rhs );
return c_lhs;
}
Vector2 Normalized() const {
Vector2 v = *this;
v.Normalize();
return v;
}
bool SpecialMonsterController::GameControllerMovement(Vector2 &obj)
{
obj.x(obj.x() - (rand() % 3));
return true;
}
void JSONValue::SetVector2(const String& name, const Vector2& value)
{
SetString(name, value.ToString());
}
void Curve2D::_bake() const {
if (!baked_cache_dirty)
return;
baked_max_ofs = 0;
baked_cache_dirty = false;
if (points.size() == 0) {
baked_point_cache.resize(0);
return;
}
if (points.size() == 1) {
baked_point_cache.resize(1);
baked_point_cache.set(0, points[0].pos);
return;
}
Vector2 pos = points[0].pos;
List<Vector2> pointlist;
pointlist.push_back(pos); //start always from origin
for (int i = 0; i < points.size() - 1; i++) {
float step = 0.1; // at least 10 substeps ought to be enough?
float p = 0;
while (p < 1.0) {
float np = p + step;
if (np > 1.0)
np = 1.0;
Vector2 npp = _bezier_interp(np, points[i].pos, points[i].pos + points[i].out, points[i + 1].pos + points[i + 1].in, points[i + 1].pos);
float d = pos.distance_to(npp);
if (d > bake_interval) {
// OK! between P and NP there _has_ to be Something, let's go searching!
int iterations = 10; //lots of detail!
float low = p;
float hi = np;
float mid = low + (hi - low) * 0.5;
for (int j = 0; j < iterations; j++) {
npp = _bezier_interp(mid, points[i].pos, points[i].pos + points[i].out, points[i + 1].pos + points[i + 1].in, points[i + 1].pos);
d = pos.distance_to(npp);
if (bake_interval < d)
hi = mid;
else
low = mid;
mid = low + (hi - low) * 0.5;
}
pos = npp;
p = mid;
pointlist.push_back(pos);
} else {
p = np;
}
}
}
Vector2 lastpos = points[points.size() - 1].pos;
float rem = pos.distance_to(lastpos);
baked_max_ofs = (pointlist.size() - 1) * bake_interval + rem;
pointlist.push_back(lastpos);
baked_point_cache.resize(pointlist.size());
PoolVector2Array::Write w = baked_point_cache.write();
int idx = 0;
for (List<Vector2>::Element *E = pointlist.front(); E; E = E->next()) {
w[idx] = E->get();
idx++;
}
}
bool CollisionPolygon2DEditor::forward_gui_input(const InputEvent& p_event) {
if (!node)
return false;
switch(p_event.type) {
case InputEvent::MOUSE_BUTTON: {
const InputEventMouseButton &mb=p_event.mouse_button;
Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
Vector2 gpoint = Point2(mb.x,mb.y);
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint=canvas_item_editor->snap_point(cpoint);
cpoint = node->get_global_transform().affine_inverse().xform(cpoint);
Vector<Vector2> poly = node->get_polygon();
//first check if a point is to be added (segment split)
real_t grab_treshold=EDITOR_DEF("editors/poly_editor/point_grab_radius",8);
switch(mode) {
case MODE_CREATE: {
if (mb.button_index==BUTTON_LEFT && mb.pressed) {
if (!wip_active) {
wip.clear();
wip.push_back( cpoint );
wip_active=true;
edited_point_pos=cpoint;
canvas_item_editor->get_viewport_control()->update();
edited_point=1;
return true;
} else {
if (wip.size()>1 && xform.xform(wip[0]).distance_to(gpoint)<grab_treshold) {
//wip closed
_wip_close();
return true;
} else {
wip.push_back( cpoint );
edited_point=wip.size();
canvas_item_editor->get_viewport_control()->update();
return true;
//add wip point
}
}
} else if (mb.button_index==BUTTON_RIGHT && mb.pressed && wip_active) {
_wip_close();
}
} break;
case MODE_EDIT: {
if (mb.button_index==BUTTON_LEFT) {
if (mb.pressed) {
if (mb.mod.control) {
if (poly.size() < 3) {
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_undo_method(node,"set_polygon",poly);
poly.push_back(cpoint);
undo_redo->add_do_method(node,"set_polygon",poly);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
return true;
}
//search edges
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int i=0;i<poly.size();i++) {
Vector2 points[2] ={ xform.xform(poly[i]),
xform.xform(poly[(i+1)%poly.size()]) };
Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint,points);
if (cp.distance_squared_to(points[0])<CMP_EPSILON2 || cp.distance_squared_to(points[1])<CMP_EPSILON2)
continue; //not valid to reuse point
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_idx=i;
}
}
if (closest_idx>=0) {
pre_move_edit=poly;
poly.insert(closest_idx+1,xform.affine_inverse().xform(closest_pos));
edited_point=closest_idx+1;
edited_point_pos=xform.affine_inverse().xform(closest_pos);
node->set_polygon(poly);
canvas_item_editor->get_viewport_control()->update();
return true;
}
} else {
//look for points to move
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int i=0;i<poly.size();i++) {
Vector2 cp =xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_idx=i;
}
}
if (closest_idx>=0) {
pre_move_edit=poly;
edited_point=closest_idx;
edited_point_pos=xform.affine_inverse().xform(closest_pos);
canvas_item_editor->get_viewport_control()->update();
return true;
}
}
} else {
if (edited_point!=-1) {
//apply
ERR_FAIL_INDEX_V(edited_point,poly.size(),false);
poly[edited_point]=edited_point_pos;
undo_redo->create_action(TTR("Edit Poly"));
undo_redo->add_do_method(node,"set_polygon",poly);
undo_redo->add_undo_method(node,"set_polygon",pre_move_edit);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
edited_point=-1;
return true;
}
}
} if (mb.button_index==BUTTON_RIGHT && mb.pressed && edited_point==-1) {
int closest_idx=-1;
Vector2 closest_pos;
real_t closest_dist=1e10;
for(int i=0;i<poly.size();i++) {
Vector2 cp =xform.xform(poly[i]);
real_t d = cp.distance_to(gpoint);
if (d<closest_dist && d<grab_treshold) {
closest_dist=d;
closest_pos=cp;
closest_idx=i;
}
}
if (closest_idx>=0) {
undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
undo_redo->add_undo_method(node,"set_polygon",poly);
poly.remove(closest_idx);
undo_redo->add_do_method(node,"set_polygon",poly);
undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
undo_redo->commit_action();
return true;
}
}
} break;
}
} break;
case InputEvent::MOUSE_MOTION: {
const InputEventMouseMotion &mm=p_event.mouse_motion;
if (edited_point!=-1 && (wip_active || mm.button_mask&BUTTON_MASK_LEFT)) {
Vector2 gpoint = Point2(mm.x,mm.y);
Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
cpoint=canvas_item_editor->snap_point(cpoint);
edited_point_pos = node->get_global_transform().affine_inverse().xform(cpoint);
canvas_item_editor->get_viewport_control()->update();
}
} break;
}
return false;
}
MeshCurvature<Real>::MeshCurvature (int iVQuantity,
const Vector3<Real>* akVertex, int iTQuantity, const int* aiConnect)
:
m_akVertex(akVertex),
m_aiConnect(aiConnect)
{
m_iVQuantity = iVQuantity;
m_iTQuantity = iTQuantity;
// compute normal vectors
m_akNormal = new Vector3<Real>[m_iVQuantity];
memset(m_akNormal,0,m_iVQuantity*sizeof(Vector3<Real>));
int i, iV0, iV1, iV2;
for (i = 0; i < m_iTQuantity; i++)
{
// get vertex indices
iV0 = *aiConnect++;
iV1 = *aiConnect++;
iV2 = *aiConnect++;
// compute the normal (length provides a weighted sum)
Vector3<Real> kEdge1 = m_akVertex[iV1] - m_akVertex[iV0];
Vector3<Real> kEdge2 = m_akVertex[iV2] - m_akVertex[iV0];
Vector3<Real> kNormal = kEdge1.Cross(kEdge2);
m_akNormal[iV0] += kNormal;
m_akNormal[iV1] += kNormal;
m_akNormal[iV2] += kNormal;
}
for (i = 0; i < m_iVQuantity; i++)
m_akNormal[i].Normalize();
// compute the matrix of normal derivatives
Matrix3<Real>* akDNormal = new Matrix3<Real>[m_iVQuantity];
Matrix3<Real>* akWWTrn = new Matrix3<Real>[m_iVQuantity];
Matrix3<Real>* akDWTrn = new Matrix3<Real>[m_iVQuantity];
memset(akWWTrn,0,m_iVQuantity*sizeof(Matrix3<Real>));
memset(akDWTrn,0,m_iVQuantity*sizeof(Matrix3<Real>));
int iRow, iCol;
aiConnect = m_aiConnect;
for (i = 0; i < m_iTQuantity; i++)
{
// get vertex indices
int aiV[3];
aiV[0] = *aiConnect++;
aiV[1] = *aiConnect++;
aiV[2] = *aiConnect++;
for (int j = 0; j < 3; j++)
{
iV0 = aiV[j];
iV1 = aiV[(j+1)%3];
iV2 = aiV[(j+2)%3];
// Compute edge from V0 to V1, project to tangent plane of vertex,
// and compute difference of adjacent normals.
Vector3<Real> kE = m_akVertex[iV1] - m_akVertex[iV0];
Vector3<Real> kW = kE - (kE.Dot(m_akNormal[iV0]))*m_akNormal[iV0];
Vector3<Real> kD = m_akNormal[iV1] - m_akNormal[iV0];
for (iRow = 0; iRow < 3; iRow++)
{
for (iCol = 0; iCol < 3; iCol++)
{
akWWTrn[iV0][iRow][iCol] += kW[iRow]*kW[iCol];
akDWTrn[iV0][iRow][iCol] += kD[iRow]*kW[iCol];
}
}
// Compute edge from V0 to V2, project to tangent plane of vertex,
// and compute difference of adjacent normals.
kE = m_akVertex[iV2] - m_akVertex[iV0];
kW = kE - (kE.Dot(m_akNormal[iV0]))*m_akNormal[iV0];
kD = m_akNormal[iV2] - m_akNormal[iV0];
for (iRow = 0; iRow < 3; iRow++)
{
for (iCol = 0; iCol < 3; iCol++)
{
akWWTrn[iV0][iRow][iCol] += kW[iRow]*kW[iCol];
akDWTrn[iV0][iRow][iCol] += kD[iRow]*kW[iCol];
}
}
}
}
// Add in N*N^T to W*W^T for numerical stability. In theory 0*0^T gets
// added to D*W^T, but of course no update needed in the implementation.
// Compute the matrix of normal derivatives.
for (i = 0; i < m_iVQuantity; i++)
{
for (iRow = 0; iRow < 3; iRow++)
{
for (iCol = 0; iCol < 3; iCol++)
{
akWWTrn[i][iRow][iCol] = ((Real)0.5)*akWWTrn[i][iRow][iCol] +
m_akNormal[i][iRow]*m_akNormal[i][iCol];
akDWTrn[i][iRow][iCol] *= (Real)0.5;
}
}
akDNormal[i] = akDWTrn[i]*akWWTrn[i].Inverse();
}
delete[] akWWTrn;
delete[] akDWTrn;
// If N is a unit-length normal at a vertex, let U and V be unit-length
// tangents so that {U, V, N} is an orthonormal set. Define the matrix
// J = [U | V], a 3-by-2 matrix whose columns are U and V. Define J^T
// to be the transpose of J, a 2-by-3 matrix. Let dN/dX denote the
// matrix of first-order derivatives of the normal vector field. The
// shape matrix is
// S = (J^T * J)^{-1} * J^T * dN/dX * J = J^T * dN/dX * J
// where the superscript of -1 denotes the inverse. (The formula allows
// for J built from non-perpendicular vectors.) The matrix S is 2-by-2.
// The principal curvatures are the eigenvalues of S. If k is a principal
// curvature and W is the 2-by-1 eigenvector corresponding to it, then
// S*W = k*W (by definition). The corresponding 3-by-1 tangent vector at
// the vertex is called the principal direction for k, and is J*W.
m_afMinCurvature = new Real[m_iVQuantity];
m_afMaxCurvature = new Real[m_iVQuantity];
m_akMinDirection = new Vector3<Real>[m_iVQuantity];
m_akMaxDirection = new Vector3<Real>[m_iVQuantity];
for (i = 0; i < m_iVQuantity; i++)
{
// compute U and V given N
Vector3<Real> kU, kV;
Vector3<Real>::GenerateOrthonormalBasis(kU,kV,m_akNormal[i],true);
// Compute S = J^T * dN/dX * J. In theory S is symmetric, but
// because we have estimated dN/dX, we must slightly adjust our
// calculations to make sure S is symmetric.
Real fS01 = kU.Dot(akDNormal[i]*kV);
Real fS10 = kV.Dot(akDNormal[i]*kU);
Real fSAvr = ((Real)0.5)*(fS01+fS10);
Matrix2<Real> kS
(
kU.Dot(akDNormal[i]*kU), fSAvr,
fSAvr, kV.Dot(akDNormal[i]*kV)
);
// compute the eigenvalues of S (min and max curvatures)
Real fTrace = kS[0][0] + kS[1][1];
Real fDet = kS[0][0]*kS[1][1] - kS[0][1]*kS[1][0];
Real fDiscr = fTrace*fTrace - ((Real)4.0)*fDet;
Real fRootDiscr = Math<Real>::Sqrt(Math<Real>::FAbs(fDiscr));
m_afMinCurvature[i] = ((Real)0.5)*(fTrace - fRootDiscr);
m_afMaxCurvature[i] = ((Real)0.5)*(fTrace + fRootDiscr);
Vector2<Real> kTest;
Real fTest;
// compute the eigenvectors of S
Vector2<Real> kW0(kS[0][1],m_afMinCurvature[i]-kS[0][0]);
Vector2<Real> kW1(m_afMinCurvature[i]-kS[1][1],kS[1][0]);
if ( kW0.SquaredLength() >= kW1.SquaredLength() )
{
kW0.Normalize();
m_akMinDirection[i] = kW0.X()*kU + kW0.Y()*kV;
kTest = kS*kW0 - m_afMinCurvature[i]*kW0;
fTest = kTest.Length();
}
else
{
kW1.Normalize();
m_akMinDirection[i] = kW1.X()*kU + kW1.Y()*kV;
kTest = kS*kW1 - m_afMinCurvature[i]*kW1;
fTest = kTest.Length();
}
kW0 = Vector2<Real>(kS[0][1],m_afMaxCurvature[i]-kS[0][0]);
kW1 = Vector2<Real>(m_afMaxCurvature[i]-kS[1][1],kS[1][0]);
if ( kW0.SquaredLength() >= kW1.SquaredLength() )
{
kW0.Normalize();
m_akMaxDirection[i] = kW0.X()*kU + kW0.Y()*kV;
kTest = kS*kW0 - m_afMaxCurvature[i]*kW0;
fTest = kTest.Length();
}
else
{
kW1.Normalize();
m_akMaxDirection[i] = kW1.X()*kU + kW1.Y()*kV;
kTest = kS*kW1 - m_afMaxCurvature[i]*kW1;
fTest = kTest.Length();
}
}
}
return relative_velocity(a, b, rA, rB).dot(n);
}
#if 0
bool PinJoint2DSW::setup(float p_step) {
Space2DSW *space = A->get_space();
ERR_FAIL_COND_V(!space,false;)
rA = A->get_transform().basis_xform(anchor_A);
rB = B?B->get_transform().basis_xform(anchor_B):anchor_B;
Vector2 gA = A->get_transform().get_origin();
Vector2 gB = B?B->get_transform().get_origin():Vector2();
Vector2 delta = gB - gA;
delta = (delta+rB) -rA;
real_t jdist = delta.length();
correct=false;
if (jdist==0)
return false; // do not correct
correct=true;
n = delta / jdist;
// calculate mass normal
mass_normal = 1.0f/k_scalar(A, B, rA, rB, n);
// calculate bias velocity
Vector4::Vector4(const Vector2 &v)
{
_vector[0] = v.x();
_vector[1] = v.y();
_vector[2] = _vector[3] = 0.0;
}
Flower::Flower(const Vector2& center)
{
const Vector2 stem_center(center.x(), center.y()+STEM_HEIGHT/2);
stem = new Rectangle(stem_center, STEM_COLOR, STEM_WIDTH, STEM_HEIGHT);
pistil = new Circle (Vector2(center.x(), center.y() - PISTIL_RADIUS/2), PISTIL_COLOR, PISTIL_RADIUS);
const Vector2 leaf_center(stem_center.x() - STEM_WIDTH/2 + 1,
stem_center.y() + STEM_HEIGHT/4);
leaf = new Triangle(LEAF_COLOR,
Vector2(leaf_center.x(), leaf_center.y() - STEM_HEIGHT/4),
Vector2(leaf_center.x() - LEAF_WIDTH, leaf_center.y() - LEAF_HEIGHT),
Vector2(leaf_center.x() - LEAF_WIDTH/2, leaf_center.y()));
}
bool PathEditorPlugin::forward_spatial_input_event(Camera* p_camera,const InputEvent& p_event) {
if (!path)
return false;
Ref<Curve3D> c=path->get_curve();
if (c.is_null())
return false;
Transform gt = path->get_global_transform();
Transform it = gt.affine_inverse();
static const int click_dist = 10; //should make global
if (p_event.type==InputEvent::MOUSE_BUTTON) {
const InputEventMouseButton &mb=p_event.mouse_button;
Point2 mbpos(mb.x,mb.y);
if (mb.pressed && mb.button_index==BUTTON_LEFT && (curve_create->is_pressed() || (curve_edit->is_pressed() && mb.mod.control))) {
//click into curve, break it down
Vector3Array v3a = c->tesselate();
int idx=0;
int rc=v3a.size();
int closest_seg=-1;
Vector3 closest_seg_point;
float closest_d=1e20;
if (rc>=2) {
Vector3Array::Read r = v3a.read();
if (p_camera->unproject_position(gt.xform(c->get_point_pos(0))).distance_to(mbpos)<click_dist)
return false; //nope, existing
for(int i=0;i<c->get_point_count()-1;i++) {
//find the offset and point index of the place to break up
int j=idx;
if (p_camera->unproject_position(gt.xform(c->get_point_pos(i+1))).distance_to(mbpos)<click_dist)
return false; //nope, existing
while(j<rc && c->get_point_pos(i+1)!=r[j]) {
Vector3 from =r[j];
Vector3 to =r[j+1];
real_t cdist = from.distance_to(to);
from=gt.xform(from);
to=gt.xform(to);
if (cdist>0) {
Vector2 s[2];
s[0] = p_camera->unproject_position(from);
s[1] = p_camera->unproject_position(to);
Vector2 inters = Geometry::get_closest_point_to_segment_2d(mbpos,s);
float d = inters.distance_to(mbpos);
if (d<10 && d<closest_d) {
closest_d=d;
closest_seg=i;
Vector3 ray_from=p_camera->project_ray_origin(mbpos);
Vector3 ray_dir=p_camera->project_ray_normal(mbpos);
Vector3 ra,rb;
Geometry::get_closest_points_between_segments(ray_from,ray_from+ray_dir*4096,from,to,ra,rb);
closest_seg_point=it.xform(rb);
}
}
j++;
}
if (idx==j)
idx++; //force next
else
idx=j; //swap
if (j==rc)
break;
}
}
UndoRedo *ur = editor->get_undo_redo();
if (closest_seg!=-1) {
//subdivide
ur->create_action("Split Path");
ur->add_do_method(c.ptr(),"add_point",closest_seg_point,Vector3(),Vector3(),closest_seg+1);
ur->add_undo_method(c.ptr(),"remove_point",closest_seg+1);
ur->commit_action();;
return true;
} else {
Vector3 org;
if (c->get_point_count()==0)
org=path->get_transform().get_origin();
else
org=gt.xform(c->get_point_pos(c->get_point_count()));
Plane p(org,p_camera->get_transform().basis.get_axis(2));
Vector3 ray_from=p_camera->project_ray_origin(mbpos);
Vector3 ray_dir=p_camera->project_ray_normal(mbpos);
Vector3 inters;
if (p.intersects_ray(ray_from,ray_dir,&inters)) {
ur->create_action("Add Point to Curve");
ur->add_do_method(c.ptr(),"add_point",it.xform(inters),Vector3(),Vector3(),-1);
ur->add_undo_method(c.ptr(),"remove_point",c->get_point_count());
ur->commit_action();;
return true;
}
//add new at pos
}
} else if (mb.pressed && ((mb.button_index==BUTTON_LEFT && curve_del->is_pressed()) || (mb.button_index==BUTTON_RIGHT && curve_edit->is_pressed()))) {
int erase_idx=-1;
for(int i=0;i<c->get_point_count();i++) {
//find the offset and point index of the place to break up
if (p_camera->unproject_position(gt.xform(c->get_point_pos(i))).distance_to(mbpos)<click_dist) {
erase_idx=i;
break;
}
}
if (erase_idx!=-1) {
UndoRedo *ur = editor->get_undo_redo();
ur->create_action("Remove Path Point");
ur->add_do_method(c.ptr(),"remove_point",erase_idx);
ur->add_undo_method(c.ptr(),"add_point",c->get_point_pos(erase_idx),c->get_point_in(erase_idx),c->get_point_out(erase_idx),erase_idx);
ur->commit_action();
return true;
}
}
}
return false;
}
/**
* Gets the absolute position of the object in local space
* @param result Storage for the position
*/
void Transform::GetLocalPosition (Vector2& result )
{
result.setX(localTransform.mat[0][2]);
result.setY(localTransform.mat[1][2]);
}
bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape,false);
Rect2 aabb = p_xform.xform(shape->get_aabb());
aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
aabb=aabb.grow(p_margin);
//if (p_motion!=Vector2())
// print_line(p_motion);
int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
float best_safe=1;
float best_unsafe=1;
for(int i=0;i<amount;i++) {
if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue; //ignore excluded
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
/*if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
if (body->get_one_way_collision_direction()!=Vector2() && p_motion.dot(body->get_one_way_collision_direction())<=CMP_EPSILON) {
print_line("failed in motion dir");
continue;
}
}*/
Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
continue;
}
//test initial overlap
if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
//if one way collision direction ignore initial overlap
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
if (body->get_one_way_collision_direction()!=Vector2()) {
continue;
}
}
return false;
}
//just do kinematic solving
float low=0;
float hi=1;
Vector2 mnormal=p_motion.normalized();
for(int i=0;i<8;i++) { //steps should be customizable..
Matrix32 xfa = p_xform;
float ofs = (low+hi)*0.5;
Vector2 sep=mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin);
if (collided) {
hi=ofs;
} else {
low=ofs;
}
}
if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
if (body->get_one_way_collision_direction()!=Vector2()) {
Vector2 cd[2];
Physics2DServerSW::CollCbkData cbk;
cbk.max=1;
cbk.amount=0;
cbk.ptr=cd;
cbk.valid_dir=body->get_one_way_collision_direction();
cbk.valid_depth=body->get_one_way_collision_max_depth();
Vector2 sep=mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*(hi+space->contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,p_margin);
if (!collided || cbk.amount==0) {
continue;
}
}
}
if (low<best_safe) {
best_safe=low;
best_unsafe=hi;
}
}
p_closest_safe=best_safe;
p_closest_unsafe=best_unsafe;
return true;
}
/**
* Gets the absolute position of the object in world space
* @param result Storage for the position
*/
void Transform::GetPosition (Vector2& result)
{
result.setX(finalTransform.mat[0][2]);
result.setY(finalTransform.mat[1][2]);
}
bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
ERR_FAIL_COND_V(space->locked,false);
Vector2 begin,end;
Vector2 normal;
begin=p_from;
end=p_to;
normal=(end-begin).normalized();
int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
bool collided=false;
Vector2 res_point,res_normal;
int res_shape;
const CollisionObject2DSW *res_obj;
real_t min_d=1e10;
for(int i=0;i<amount;i++) {
if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
Vector2 local_from = inv_xform.xform(begin);
Vector2 local_to = inv_xform.xform(end);
/*local_from = col_obj->get_inv_transform().xform(begin);
local_from = col_obj->get_shape_inv_transform(shape_idx).xform(local_from);
local_to = col_obj->get_inv_transform().xform(end);
local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
const Shape2DSW *shape = col_obj->get_shape(shape_idx);
Vector2 shape_point,shape_normal;
if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
shape_point=xform.xform(shape_point);
real_t ld = normal.dot(shape_point);
if (ld<min_d) {
min_d=ld;
res_point=shape_point;
res_normal=inv_xform.basis_xform_inv(shape_normal).normalized();
res_shape=shape_idx;
res_obj=col_obj;
collided=true;
}
}
}
if (!collided)
return false;
r_result.collider_id=res_obj->get_instance_id();
if (r_result.collider_id!=0)
r_result.collider=ObjectDB::get_instance(r_result.collider_id);
r_result.normal=res_normal;
r_result.metadata=res_obj->get_shape_metadata(res_shape);
r_result.position=res_point;
r_result.rid=res_obj->get_self();
r_result.shape=res_shape;
return true;
}
bool Vector2::operator<(const Vector2& v)
{
return length() < v.length();
}
Vector2 Add( Vector2 c_lhs, Vector2 const& i_rhs )
{
c_lhs.Add( i_rhs );
return c_lhs;
}
