@Test public void testCollision() { Pose pos = Pose.zero(); IVector3 vel = Vector3.zero(); double center2Dribbler = 75; double obsRadius = getBotRadius() + getBallRadius(); double obsFront = center2Dribbler + getBallRadius(); BotID botID = BotID.createBotId(1, ETeamColor.YELLOW); BotCollisionObject colHandler = new BotCollisionObject(pos, vel, center2Dribbler, botID); Optional<ICollision> collision; collision = colHandler.getCollision(Vector3.fromXY(0, 0), Vector3.fromXY(0, 0)); assertThat(collision).isNotPresent(); collision = colHandler.getCollision(Vector3.fromXY(-120, 0), Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(-obsRadius, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(-1, 0)); collision = colHandler.getCollision(Vector3.fromXY(-obsRadius, 0), Vector3.fromXY(-obsRadius, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(-obsRadius, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(-1, 0)); collision = colHandler.getCollision(Vector3.fromXY(-120, 0), Vector3.fromXY(-119, 0)); assertThat(collision).isNotPresent(); collision = colHandler.getCollision(Vector3.fromXY(0, -120), Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(0, -obsRadius)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(0, -1)); collision = colHandler.getCollision(Vector3.fromXY(120, 0), Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getCollision(Vector3.fromXY(obsFront, 0), Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getCollision(Vector3.fromXY(obsFront, 0), Vector3.fromXY(obsFront, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getCollision(Vector3.fromXY(obsFront + 1, 1), Vector3.fromXY(obsFront, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getCollision(Vector3.fromXY(obsFront + 1, 1), Vector3.fromXY(obsFront - 1, -1)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getCollision(Vector3.fromXY(obsFront + 1, 2), Vector3.fromXY(obsFront - 1, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 1)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); double theta = acos(center2Dribbler / getBotRadius()) + 0.001; IVector2 mostInnerCircleColPre = Vector2.fromAngle(theta).scaleTo(120); collision = colHandler.getCollision(Vector3.from2d(mostInnerCircleColPre, 0), Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos().subtractNew(Vector2.fromAngle(theta).scaleTo(obsRadius)).getLength2()) .isCloseTo(0, within(0.1)); assertThat(collision.get().getNormal().getAngle()).isEqualTo(theta, within(0.001)); double theta2 = theta - 0.01; IVector2 outerLineColPre = Vector2.fromAngle(theta2).scaleTo(120); collision = colHandler.getCollision(Vector3.from2d(outerLineColPre, 0), Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos().x()).isCloseTo(obsFront, within(1e-5)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromX(1)); }
@Override public Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos) { if (isInFront(prePos)) { return lineCollision.getCollision(prePos, postPos); } return circleCollisionObject.getCollision(prePos, postPos); }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos) { if (isInFront(prePos)) { return lineCollision.getCollision(prePos, postPos); } return circleCollisionObject.getCollision(prePos, postPos); } }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos) { if (isInFront(prePos)) { return lineCollision.getCollision(prePos, postPos); } return circleCollisionObject.getCollision(prePos, postPos); } BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID);  BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID,
			final boolean sticky,
			final double kickSpeed, final boolean chip); }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos) { if (isInFront(prePos)) { return lineCollision.getCollision(prePos, postPos); } return circleCollisionObject.getCollision(prePos, postPos); } BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID);  BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID,
			final boolean sticky,
			final double kickSpeed, final boolean chip); @Override IVector3 getVel(); @Override Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos); @Override Optional<ICollision> getInsideCollision(final IVector3 prePos); @Override IVector3 getImpulse(final IVector3 prePos); @Override BotID getBotID(); }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos) { if (isInFront(prePos)) { return lineCollision.getCollision(prePos, postPos); } return circleCollisionObject.getCollision(prePos, postPos); } BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID);  BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID,
			final boolean sticky,
			final double kickSpeed, final boolean chip); @Override IVector3 getVel(); @Override Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos); @Override Optional<ICollision> getInsideCollision(final IVector3 prePos); @Override IVector3 getImpulse(final IVector3 prePos); @Override BotID getBotID();  }