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;
            }
        }
//.........这里部分代码省略.........

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();

//.........这里部分代码省略.........

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();

//.........这里部分代码省略.........

 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;
        }
      }
    }
//.........这里部分代码省略.........

 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
//.........这里部分代码省略.........

//.........这里部分代码省略.........
            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