/**
* Places the car. The car has to follow the ground. This is done by casting 4 rays (one from each
* wheel) to the ground and calculating the resulting rotation angles to let the car follow the
* ground as close as possible.
*
* @param ground the ground
* @return if it was possible to place the car or not
*/
public boolean place(Object3D ground) {
SimpleVector dropDown = new SimpleVector(0, 1, 0);
/**
* To cast the rays, the car will be rotated in horizontal position first, rotated around the
* y-axis according to the cars direction and moved 10 units up.
*/
Matrix rotMat = getRotationMatrix();
rotMat.setIdentity();
setRotationMatrix(rotMat);
rotateY(yRot);
translate(0, -10, 0);
/** Cast the rays... */
float rightFrontHeight =
ground.calcMinDistance(rightFront.getTransformedCenter(), dropDown, 4 * 30);
float rightRearHeight =
ground.calcMinDistance(rightRear.getTransformedCenter(), dropDown, 4 * 30);
float leftFrontHeight =
ground.calcMinDistance(leftFront.getTransformedCenter(), dropDown, 4 * 30);
float leftRearHeight =
ground.calcMinDistance(leftRear.getTransformedCenter(), dropDown, 4 * 30);
/** Correct the movement we did above. */
translate(0, 10, 0);
/** Reset the rotation matrix again. */
rotMat = getRotationMatrix();
rotMat.setIdentity();
setRotationMatrix(rotMat);
/** The rays all hit the ground, the car can be placed */
if (rightFrontHeight != Object3D.COLLISION_NONE
&& rightRearHeight != Object3D.COLLISION_NONE
&& leftFrontHeight != Object3D.COLLISION_NONE
&& leftRearHeight != Object3D.COLLISION_NONE) {
/** Correct the values (see translation above) */
rightFrontHeight -= 10;
rightRearHeight -= 10;
leftFrontHeight -= 10;
leftRearHeight -= 10;
/**
* Calculate the angles between the wheels and the ground. This is done four times: for the
* front and the rear as well as for left and right. Front/rear and left/right are averaged
* afterwards.
*/
double angleFront = rightFrontHeight - leftFrontHeight;
double as = (angleFront / (16d));
angleFront = Math.atan(as);
double angleRear = rightRearHeight - leftRearHeight;
as = (angleRear / (16d));
angleRear = Math.atan(as);
float rot = (float) ((angleFront + angleRear) / 2d);
rotateZ(rot);
double angleLeft = leftFrontHeight - leftRearHeight;
as = (angleLeft / (16d));
angleLeft = Math.atan(as);
double angleRight = rightFrontHeight - rightRearHeight;
as = (angleRight / (16d));
angleRight = Math.atan(as);
rot = (float) ((angleLeft + angleRight) / 2d);
rotateX(rot);
/**
* The car is correctly rotated now. But we still have to adjust the height. We are simply
* taking the minimum distance from all wheels to the ground as the new height.
*/
float down = rightFrontHeight;
if (leftFrontHeight < down) {
down = leftFrontHeight;
}
if (rightRearHeight < down) {
down = rightRearHeight;
}
if (leftRearHeight < down) {
down = leftRearHeight;
}
dropDown.scalarMul(down - 4);
translate(dropDown);
} else {
return false;
}
/** And finally, rotate the car around Y (that's the car's direction) */
rotateY(yRot);
return true;
}
