- float dx = (crod.position.x+i->type->connect_offset.x)-i->position.x;
- float dz = (crod.position.z+i->type->connect_offset.z)-i->position.z;
- float cd = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z);
- float ca = atan2(i->type->connect_point.z, i->type->connect_point.x);
- dx = sqrt(cd*cd-dz*dz)*(dx>0 ? 1 : -1);
- i->angle = atan2(dz, dx)-ca;
- crod.position.x = i->position.x+dx-i->type->connect_offset.x;
- }
- else if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::ROTATE)
- {
- float dx = crod.position.x-i->position.x;
- float dz = crod.position.z-i->position.z;
- float d = sqrt(dx*dx+dz*dz);
- float cd1 = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z);
- float cd2 = sqrt(i->type->connect_offset.x*i->type->connect_offset.x+i->type->connect_offset.z*i->type->connect_offset.z);
- float a = (d*d+cd1*cd1-cd2*cd2)/(2*d);
- float b = sqrt(cd1*cd1-a*a);
- float sign = (dx*i->type->connect_point.z-dz*i->type->connect_point.x>0 ? 1 : -1);
- float cx = (dx*a-dz*b*sign)/d;
- float cz = (dz*a+dx*b*sign)/d;
- float ca1 = atan2(i->type->connect_point.z, i->type->connect_point.x);
- float ca2 = atan2(i->type->connect_offset.z, i->type->connect_offset.x);
- i->angle = atan2(cz, cx)-ca1;
- crod.angle = atan2(cz-dz, cx-dx)-ca2;