function trilateration3d($p1,$p2,$p3,$p4){
$ex = vector_unit(vector_diff($p2, $p1));
$i = vector_dot_product($ex, vector_diff($p3, $p1));
$ey = vector_unit(vector_diff(vector_diff($p3, $p1), vector_multiply($ex, $i)));
$ez = vector_cross($ex,$ey);
$d = vector_length($p2, $p1);
$r1 = $p1[3]; $r2 = $p2[3]; $r3 = $p3[3]; $r4 = $p4[3];
if($d - $r1 >= $r2 || $r2 >= $d + $r1){
return array();
}
$j = vector_dot_product($ey, vector_diff($p3, $p1));
$x = (($r1*$r1) - ($r2*$r2) + ($d*$d)) / (2*$d);
$y = ((($r1*$r1) - ($r3*$r3) + ($i*$i) + ($j*$j)) / (2*$j)) - (($i*$x) / $j);
$z = $r1*$r1 - $x*$x - $y*$y;
if($z < 0){
return array();
}
$z1 = sqrt($z);
$z2 = $z1 * -1;
$result1 = $p1;
$result1 = vector_sum($result1, vector_multiply($ex, $x));
$result1 = vector_sum($result1, vector_multiply($ey, $y));
$result1 = vector_sum($result1, vector_multiply($ez, $z1));
$result2 = $p1;
$result2 = vector_sum($result2, vector_multiply($ex, $x));
$result2 = vector_sum($result2, vector_multiply($ey, $y));
$result2 = vector_sum($result2, vector_multiply($ez, $z2));
$r1 = vector_length($p4, $result1);
$r2 = vector_length($p4, $result2);
$t1 = $r1 - $r4;
$t2 = $r2 - $r4;
$coords = array();
if(abs($t1) < abs($t2)){
$coords = array((round(($result1[0]*32),0)/32), (round(($result1[1]*32),0)/32), (round(($result1[2]*32),0)/32));
}
else{
$coords = array((round(($result2[0]*32),0)/32), (round(($result2[1]*32),0)/32), (round(($result2[2]*32),0)/32));
}
return $coords;
}