public Vector3 IsColliedVec(SBB other)
{
Vector3 dif = Center - other.Center;
float r_t = Radius + other.Radius;
return(dif.normalized * (r_t - dif.magnitude));
}
public bool IsCollied(SBB other)
{
float d2 = Vector3.Dot(Center, other.Center);//距离和的平方
float r_t = Radius + other.Radius;
if (d2 < r_t * r_t)
{
return(true);
}
return(false);
}
public List <SBB> GetSBB()
{
List <SBB> sbbs = new List <SBB>();
foreach (var item in balls)
{
var pos = ToVector3;
SBB newsbb = new SBB(pos + item.Center, item.Radius);
sbbs.Add(newsbb);
}
return(sbbs);
}
public bool IsCollision(SBB tool, SBB obj)
{
float x_dif = tool.Center.x - obj.Center.x;
float y_dif = tool.Center.y - obj.Center.y;
float z_dif = tool.Center.z - obj.Center.z;
float r_t = tool.Radius + obj.Radius;
if (x_dif * x_dif + y_dif * y_dif + z_dif * z_dif < r_t * r_t)
{
return(true);
}
return(false);
}
public void TreeNodeCollison(SBB penSBB, TreeNode <SBB> treeNode)
{
if (penSBB.IsCollied(treeNode.Data)) //碰撞了，才能执行子节点的碰撞检测
{
if (treeNode.ChildNodes.Count == 0) //说明是最后一个节点，可以计算力觉
{
direction += penSBB.IsColliedVec(treeNode.Data); //力觉方向
// Debug.Log(direction.ToString());
}
else
{
var treechildnodes = treeNode.ChildNodes;
foreach (var childnode in treechildnodes)
{
TreeNodeCollison(penSBB, childnode);
}
}
}
}
// Start is called before the first frame update
void Start()
{
SBB headSbb = new SBB(new Vector3(-0.186f, 0, -0.286f), 1.8f / 2);
SBB childSbb1 = new SBB(Vector3.zero, 1 / 2);
TreeNode <SBB> childtree1 = new TreeNode <SBB>(childSbb1);
SBB childSbb2 = new SBB(new Vector3(0, 0, -0.3f), 1.2f / 2);
TreeNode <SBB> childtree2 = new TreeNode <SBB>(childSbb2);
SBB childSbb3 = new SBB(new Vector3(-0.4f, 0, 0), 1 / 2);
TreeNode <SBB> childtree3 = new TreeNode <SBB>(childSbb3);
SBB childSbb4 = new SBB(new Vector3(-0.4f, 0.1f, -0.6f), 0.8f / 2);
TreeNode <SBB> childtree4 = new TreeNode <SBB>(childSbb4);
List <TreeNode <SBB> > treeNodes = new List <TreeNode <SBB> >
{
childtree1,
childtree2,
childtree3,
childtree4
};
head_obj = new TreeNode <SBB>(headSbb, treeNodes);
Vector gtt = ToVector(Pen.transform.position, Vector3.zero);
var trans = Pen.GetComponentsInChildren <Transform>();
List <SBB> penspheresbb = new List <SBB>();
int i = 0;
foreach (var item in trans)
{
i++;
if (i == 1)
{
continue; //第一个为自己，删除
}
SBB pensphere = new SBB(item.localPosition, item.localScale.x / 2);
Debug.Log(pensphere.Center.ToString() + " " + pensphere.Radius);
penspheresbb.Add(pensphere);
}
head_pen = new PenSphere(gtt, penspheresbb);
}
