public void process(long ticks, Level level) {
if (exploding) {
checkChild();
float mul = 0.05f;
if (!botUsage) {
child.animate(animIndex);
child.getTranslationMatrix().setIdentity();
child.translate(0, 8f * animIndex, 0);
SimpleVector sz = child.getZAxis();
sz.scalarMul(4f * animIndex);
child.translate(sz);
}
animIndex += ((float) ticks) * mul;
if (animIndex >= 1 && exploding != false) {
exploding = false;
if (!botUsage) {
child.setRealVisibility(false);
}
}
if (animIndex >= 0.5f && !itemShown) {
// The item inherits the id from the crate, which is unique and
// the same on all clients.
if (level.createItem(getTransformedCenter(), getLocalObject(), sh) && !botUsage) {
SoundManager.getInstance().play("bubble", getTranslation());
}
itemShown = true;
}
}
}
@Override
public String printObject() {
try {
final LispThread thread = LispThread.currentThread();
// FIXME
if (typep(Symbol.RESTART) != NIL) {
Symbol PRINT_RESTART = PACKAGE_SYS.intern("PRINT-RESTART");
LispObject fun = PRINT_RESTART.getSymbolFunction();
StringOutputStream stream = new StringOutputStream();
thread.execute(fun, this, stream);
return stream.getString().getStringValue();
}
if (_PRINT_STRUCTURE_.symbolValue(thread) == NIL)
return unreadableString(structureClass.getName().printObject());
int maxLevel = Integer.MAX_VALUE;
LispObject printLevel = Symbol.PRINT_LEVEL.symbolValue(thread);
if (printLevel instanceof Fixnum) maxLevel = ((Fixnum) printLevel).value;
LispObject currentPrintLevel = _CURRENT_PRINT_LEVEL_.symbolValue(thread);
int currentLevel = Fixnum.getValue(currentPrintLevel);
if (currentLevel >= maxLevel && slots.length > 0) return "#";
StringBuilder sb = new StringBuilder("#S(");
sb.append(structureClass.getName().printObject());
if (currentLevel < maxLevel) {
LispObject effectiveSlots = structureClass.getSlotDefinitions();
LispObject[] effectiveSlotsArray = effectiveSlots.copyToArray();
Debug.assertTrue(effectiveSlotsArray.length == slots.length);
final LispObject printLength = Symbol.PRINT_LENGTH.symbolValue(thread);
final int limit;
if (printLength instanceof Fixnum)
limit = Math.min(slots.length, ((Fixnum) printLength).value);
else limit = slots.length;
final boolean printCircle = (Symbol.PRINT_CIRCLE.symbolValue(thread) != NIL);
for (int i = 0; i < limit; i++) {
sb.append(' ');
SimpleVector slotDefinition = (SimpleVector) effectiveSlotsArray[i];
// FIXME AREF(1)
LispObject slotName = slotDefinition.AREF(1);
Debug.assertTrue(slotName instanceof Symbol);
sb.append(':');
sb.append(((Symbol) slotName).name.getStringValue());
sb.append(' ');
if (printCircle) {
StringOutputStream stream = new StringOutputStream();
thread.execute(Symbol.OUTPUT_OBJECT.getSymbolFunction(), slots[i], stream);
sb.append(stream.getString().getStringValue());
} else sb.append(slots[i].printObject());
}
if (limit < slots.length) sb.append(" ...");
}
sb.append(')');
return sb.toString();
} catch (StackOverflowError e) {
error(new StorageCondition("Stack overflow."));
return null; // Not reached.
}
}
protected int getSlotIndex(LispObject slotName) {
LispObject effectiveSlots = structureClass.getSlotDefinitions();
LispObject[] effectiveSlotsArray = effectiveSlots.copyToArray();
for (int i = 0; i < slots.length; i++) {
SimpleVector slotDefinition = (SimpleVector) effectiveSlotsArray[i];
LispObject candidateSlotName = slotDefinition.AREF(1);
if (slotName == candidateSlotName) {
return i;
}
}
return -1;
}
@Override
public LispObject getParts() {
LispObject result = NIL;
result = result.push(new Cons("class", structureClass));
LispObject effectiveSlots = structureClass.getSlotDefinitions();
LispObject[] effectiveSlotsArray = effectiveSlots.copyToArray();
Debug.assertTrue(effectiveSlotsArray.length == slots.length);
for (int i = 0; i < slots.length; i++) {
SimpleVector slotDefinition = (SimpleVector) effectiveSlotsArray[i];
LispObject slotName = slotDefinition.AREF(1);
result = result.push(new Cons(slotName, slots[i]));
}
return result.nreverse();
}
/**
* Method to calculate alpha and t as indices from the radon transformed image of a view.
* Neccessary are the inverse projection image (as SimpleMatrix) and the epipolar plane E
*
* @param E: epipolar plane E as SimpleVector (4 entries)
* @return: line integral value as double
*/
private double getValueByAlphaAndT(SimpleVector E) {
// compute corresponding epipolar lines //
// (epipolar lines are of 3x1 = 3x4 * 4x1)
SimpleVector l_kappa = SimpleOperators.multiply(this.P_Inverse.transposed(), E);
// init the coordinate shift //
int t_u = this.projectionWidth / 2;
int t_v = this.projectionHeight / 2;
// compute angle alpha and distance to origin t //
double l0 = l_kappa.getElement(0);
double l1 = l_kappa.getElement(1);
double l2 = l_kappa.getElement(2);
double alpha_kappa_RAD = Math.atan2(-l0, l1) + Math.PI / 2;
double t_kappa = -l2 / Math.sqrt(l0 * l0 + l1 * l1);
// correct the coordinate shift //
t_kappa -= t_u * Math.cos(alpha_kappa_RAD) + t_v * Math.sin(alpha_kappa_RAD);
// correct some alpha falling out of the radon window //
if (alpha_kappa_RAD < 0) {
alpha_kappa_RAD *= -1.0;
} else if (alpha_kappa_RAD > Math.PI) {
alpha_kappa_RAD = 2.0 * Math.PI - alpha_kappa_RAD;
}
// write back to l_kappa //
l_kappa.setElementValue(0, Math.cos(alpha_kappa_RAD));
l_kappa.setElementValue(1, Math.sin(alpha_kappa_RAD));
l_kappa.setElementValue(2, -t_kappa);
// calculate x and y coordinates for derived radon transformed image //
double x = t_kappa * this.lineIncrement + 0.5 * this.radonWidth;
double y = alpha_kappa_RAD * this.angleIncrement;
// get intensity value out of radon transformed image //
// (interpolation needed)
return InterpolationOperators.interpolateLinear(this.radon, x, y);
}
/**
* 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;
}
/**
* method to calculate a mapping K from two source positions C0, C1 to a plane C0 (C1) is the
* source position from the first (second) view
*/
public void createMappingToEpipolarPlane() {
// set up source matrices //
SimpleVector C0 = this.view1.C;
SimpleVector C1 = this.view2.C;
// compute Pluecker coordinates //
double L01 = C0.getElement(0) * C1.getElement(1) - C0.getElement(1) * C1.getElement(0);
double L02 = C0.getElement(0) * C1.getElement(2) - C0.getElement(2) * C1.getElement(0);
double L03 = C0.getElement(0) * C1.getElement(3) - C0.getElement(3) * C1.getElement(0);
double L12 = C0.getElement(1) * C1.getElement(2) - C0.getElement(2) * C1.getElement(1);
double L13 = C0.getElement(1) * C1.getElement(3) - C0.getElement(3) * C1.getElement(1);
double L23 = C0.getElement(2) * C1.getElement(3) - C0.getElement(3) * C1.getElement(2);
// construct B (6x1) //
SimpleVector B = new SimpleVector(L01, L02, L03, L12, L13, L23);
// compute infinity point in direction of B //
SimpleVector N = new SimpleVector(-L03, -L13, -L23, 0);
// compute plane E0 containing B and X0=(0,0,0,1) //
SimpleVector E0 = SimpleOperators.getPlueckerJoin(B, new SimpleVector(0, 0, 0, 1));
// find othonormal basis from plane normals //
// (vectors are of 3x1)
SimpleVector a2 = new SimpleVector(E0.getElement(0), E0.getElement(1), E0.getElement(2));
SimpleVector a3 = new SimpleVector(N.getElement(0), N.getElement(1), N.getElement(2));
// set vectors to unit length
a2.normalizeL2();
a3.normalizeL2();
// calculate cross product to get the last basis vector //
SimpleVector a1 = General.crossProduct(a2, a3).negated();
// (a1 is already of unit length -> no normalization needed)
// set up assembly matrix A (4x3) //
SimpleMatrix A = new SimpleMatrix(4, 3);
A.setSubColValue(0, 0, a1);
A.setSubColValue(0, 1, a2);
A.setSubColValue(0, 2, C0);
// store mapping matrix K (4x3 = 4x4 * 4x3) //
this.K = SimpleOperators.multiplyMatrixProd(SimpleOperators.getPlueckerMatrixDual(B), A);
}
