3D Fast Geometric Collision Avoidance Algorithm (FGA) and Decision-Making Approach based on the Balance of Safety and Cost for UAS