This repository was archived by the owner on Jun 3, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathtools.py
executable file
·147 lines (129 loc) · 5.41 KB
/
tools.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon Dec 28 20:29:59 2020
This file defines some useful planar constraints
@author: tjards
"""
import numpy as np
def buildWall(wType, pos):
if wType == 'horizontal':
# define 3 points on the plane (this one is horizontal)
wallp1 = np.array([0, 0, pos])
wallp2 = np.array([5, 10, pos])
wallp3 = np.array([20, 30, pos+0.05])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
if wType == 'vertical1':
# define 3 points on the plane (this one is vertical
wallp1 = np.array([0, pos, 0])
wallp2 = np.array([5, pos, 10])
wallp3 = np.array([20,pos+0.05, 30])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
if wType == 'vertical2':
# define 3 points on the plane (this one is vertical
wallp1 = np.array([pos, 0, 0])
wallp2 = np.array([pos, 5, 10])
wallp3 = np.array([pos+0.05, 20, 30])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
if wType == 'diagonal1a':
# define 3 points on the plane (this one is vertical
wallp1 = np.array([0, pos, 0])
wallp2 = np.array([0, pos+5, 5])
wallp3 = np.array([-5,pos+5, 5])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
if wType == 'diagonal1b':
# define 3 points on the plane (this one is vertical
wallp1 = np.array([0, pos, 0])
wallp2 = np.array([0, pos-5, 5])
wallp3 = np.array([-5,pos-5, 5])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
if wType == 'diagonal2a':
# define 3 points on the plane (this one is vertical
wallp1 = np.array([pos, 0, 0])
wallp2 = np.array([pos-5, 0, 5])
wallp3 = np.array([pos-5, -5, 5])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
if wType == 'diagonal2b':
# define 3 points on the plane (this one is vertical
wallp1 = np.array([pos, 0, 0])
wallp2 = np.array([pos+5, 0, 5])
wallp3 = np.array([pos+5, -5, 5])
# define two vectors on the plane
v1 = wallp3 - wallp1
v2 = wallp2 - wallp1
# compute vector normal to the plane
wallcp = np.cross(v1, v2)
walla, wallb, wallc = wallcp
walld = np.dot(wallcp, wallp3)
walls = np.zeros((6,1))
walls[0:3,0] = np.array(wallcp, ndmin=2)#.transpose()
walls[3:6,0] = np.array(wallp1, ndmin=2)#.transpose()
walls_plots = np.zeros((4,1))
walls_plots[:,0] = np.array([walla, wallb, wallc, walld])
return walls, walls_plots