File indexing completed on 2025-04-27 04:01:15
0001 from..nvector import NVector 0002 0003 0004 # FABRIK 0005 class Chain: 0006 def __init__(self, tail, fixed_tail=True, tolerance=0.5, max_iter=8): 0007 self.joints = [tail.clone()] 0008 self.fixed_tail = fixed_tail 0009 self.lengths = [] 0010 self.total_length = 0 0011 self.tolerance = tolerance 0012 self.max_iter = max_iter 0013 0014 def add_joint(self, point): 0015 length = (point - self.joints[-1]).length 0016 self.lengths.append(length) 0017 self.total_length += length 0018 self.joints.append(point.clone()) 0019 0020 def add_joints(self, head, n): 0021 delta = head - self.joints[-1] 0022 self.total_length += delta.length 0023 segment = delta / n 0024 seglen = segment.length 0025 for i in range(n): 0026 self.lengths.append(seglen) 0027 self.joints.append(self.joints[-1] + segment) 0028 0029 def backward(self, target): 0030 """! 0031 target -> -> start 0032 """ 0033 self.joints[-1] = target 0034 for i in range(len(self.joints)-2, -1, -1): 0035 r = self.joints[i+1] - self.joints[i] 0036 l = self.lengths[i] / r.length 0037 self.joints[i] = self.joints[i+1].lerp(self.joints[i], l) 0038 0039 def forward(self, target): 0040 """! 0041 start -> -> tail 0042 """ 0043 self.joints[0] = target 0044 for i in range(0, len(self.joints)-1): 0045 r = self.joints[i+1] - self.joints[i] 0046 l = self.lengths[i] / r.length 0047 self.joints[i+1] = self.joints[i].lerp(self.joints[i+1], l) 0048 0049 def reach(self, target): 0050 if not self.fixed_tail: 0051 self.backward(target) 0052 return 0053 0054 distance = (target - self.joints[0]).length 0055 if distance >= self.total_length: 0056 for i in range(len(self.joints)-1): 0057 r = target - self.joints[i] 0058 l = self.lengths[i] / r.length 0059 self.joints[i+1] = self.joints[i].lerp(target, l) 0060 return 0061 0062 base = self.joints[0] 0063 0064 distance = (target - self.joints[-1]).length 0065 n_it = 0 0066 while distance > self.tolerance and n_it < self.max_iter: 0067 self.backward(target) 0068 self.forward(base) 0069 distance = (target - self.joints[-1]).length 0070 n_it += 1 0071 0072 0073 class Octopus: 0074 def __init__(self, master): 0075 self.chains = {"master": master} 0076 self.master = master 0077 0078 @property 0079 def base(self): 0080 return self.master.joints[-1] 0081 0082 def add_chain(self, name): 0083 ch = Chain(self.base) 0084 self.chains[name] = ch 0085 return ch 0086 0087 def reach(self, target_map): 0088 centroid = NVector(0, 0) 0089 for chain, target in target_map.items(): 0090 self.chains[chain].backward(target) 0091 centroid += self.chains[chain].joints[0] 0092 centroid /= len(target_map) 0093 0094 self.master.reach(centroid) 0095 0096 for chain in target_map.keys(): 0097 self.chains[chain].forward(self.base)