Hi, I want to make an AI routine that keeps P3 away from the ‘wall’ made by P1 and P2. To do this, I want to find the vector, whose length is L2, and direction is perpendicular to the line L1. This must work on both sides of the wall.
I made an implementation of this, but it is long and slow. I was wondering if there was a quicker way to do this.
Here’s my code in case anyone wants to look:
halfpi= pi/2
class StarlingWall:
def __init__(self,StarlingA, StarlingB):
self.sap=StarlingA.getPos()
self.sbp=StarlingB.getPos()
self.vec1=sap-sbp
self.vec3=sbp-sap
self.vec1.normalize()
self.vec3.normalize()
def getVector(self, StarlingX):
sxp=StarlingX.getPos()
vec2=sap-sxp
vec4=sbp-sxp
vec2.normalize()
vec4.normalize()
angle = vec1.signedAngleRad(vec2, Vec3(0,0,-1))
angle2= vec3.signedAngleRad(vec4, Vec3(0,0,-1)) #OPT: rem signed angle2
#DON'T RETURN IF StarlingX IS PAST StarlingA OR StarlingB
if (abs(angle)<halfpi) and (abs(angle2)<halfpi): #OPT: rem abs(ang2))
vecA=sxp-sap
lenNormal=sin(angle)*vecA.length()
normal=vec1.cross(Vec3(0,0,-1))
normal=normal * lenNormal
return normal
this is a little different.
I use the length from the wall and check a min val
Also calculation of the normal is just using the
perpendicular vector between P1 and P2, of course it assumes you move in the xy plane an z is up
this isn’t tested
class StarlingWall:
def __init__(self,StarlingA, StarlingB):
self.sap=StarlingA.getPos()
self.vec1= StarlingB - StarlingA
self.vec1.normalize()
self.minVal = .1
def getVector(self, StarlingX):
vec2=StarlingX.getPos() - sap
angle = vec1.signedAngleRad(vec2, Vec3(0,0,-1))
lenNormal = vec2.length() * sin(angle)
#DON'T RETURN IF lenNormal less than some min value
if (abs(lenNormal) > minValue):
Vec3 normal(-vec1.getY(), vec1.getX(), vec1.getZ())
normal = normal * lenNormal
return normal
def midpoint(a, b):
return (a[0] + b[0]) * 0.5, (a[1] + b[1]) * 0.5, (a[2] + b[2]) * 0.5
def rotate_point(angle, x, z):
s = math.sin(angle)
c = math.cos(angle)
return ((x * c) - (z * s), (x * s) + (z * c))
def coverpoint(p1, p2, radius): # (Z-Up version)
p1 = midpoint(p1, p2)
x = p1[0] - p2[0]
z = p1[2] - p2[2]
l = math.sqrt(x*x + z*z)
a = math.atan2(z, x)
p = rotate_point(a, l, radius)
return (p[0] + p2[0], p1[1], p[1] + p2[2])
def coverpoint(p1, p2, radius): # (Y-Up version)
p1 = midpoint(p1, p2)
x = p1[0] - p2[0]
y = p1[1] - p2[1]
l = math.sqrt(x*x + y*y)
a = math.atan2(y, x)
p = rotate_point(a, l, radius)
return (p[0] + p2[0], p[1] + p2[1], p1[2])
I use it for when I just have the points to work with. You can be faster if you cache some info ahead of time. For example if you know the length of the line already you can cut out the square root.
If you want the point on the opposite side of the line switch p1 and p2 passed to the coverpoint function. Radius would be your “L2”.
This is my take. Should work in 3D, and it considers the case when the normal doesn’t hit the short wall.
Actually here we’re not talking about a wall, but more likely a rope… For a wall, I’d suggest to define it with 3 points.
class StarlingWall:
def __init__(self,A,B):
self.min= 0.1 #your choice
setWall(self,A,B)
self.build = False;
def setWall(self,A,B):
self.P1 = A
self.P2 = B
self.Wall = B.getPos()-A.getPos()
if (self.Wall.length() < self.minWall):
# you'd better consider the wall undefined!
self.build = False;
print 'too short'
return False
self.vecWall = self.Wall
self.vecWall.normalize()
self.build = True;
return True
def distanceToWall(self, X):
vecAX= X.getPos() - self.P1.getPos()
P1ToNormalFoot = vecAX.dot(self.vecWall)
if (P1ToNormalFoot<0) || (P1ToNormalFoot >self.Wall.length()):
# P3 can go straight on, it won't hit the wall
lenNormal = -1;
else:
lenNormal = math.sqrt(vecAX.length*vecAX.length -P1ToNormalFoot*P1ToNormalFoot)
return lenNormal
It is really much more simple than that. Please do not use Python’s math modules, using Panda’s vector math functions is much faster. And don’t even try to use trigonometry for something as simple as this!
The vector you’re looking for is simply described as p3 minus the point on l1 that is closest to p3.
Should be something like this (didn’t test it, be sure to double-check my math):
# Assuming p1, p2 and p3 are of type Point3
l1 = p2 - p1
proj = (p3 - p1).project(l1)
closest = Point3(p1 + proj)
l2 = p3 - closest
Or as one-liner:
l2 = p3 - (p1 + (p3 - p1).project(p2 - p1))
Might want to add this the first time to double-check that they are perpendicular:
assert l2.dot(l1) == 0, "rdb failed at linear algebra"
l2 will be the vector you’re looking for, and thus the shortest distance between p3 and the line will be l2.length().
Please specify what exactly you mean by “between”. Do you mean to check if p3 lies exactly on the (infinitely thin) line between p1 and p2?
Or do you mean to ask if the closest point on the l1 lies between p1 and p2? In that case, I’d actually come up with the same solution as you did.
For the record, the technique I gave you should work for any location of p1 and p2 on the line l1, it also works outside those points (assumes l1 is an infinite line).
