| /******************************************************************************* |
| * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency |
| * All rights reserved. This program and the accompanying materials |
| * are made available under the terms of the Eclipse Public License v1.0 |
| * which accompanies this distribution, and is available at |
| * http://www.eclipse.org/legal/epl-v10.html |
| * |
| * Contributors: |
| * Pierre Allard, |
| * Regent L'Archeveque, |
| * Sebastien Gemme - initial API and implementation |
| * |
| * SPDX-License-Identifier: EPL-1.0 |
| * |
| *******************************************************************************/ |
| package org.eclipse.apogy.core.impl; |
| |
| import javax.vecmath.Matrix4d; |
| import javax.vecmath.Point3d; |
| import javax.vecmath.Vector3d; |
| |
| import org.eclipse.apogy.common.emf.transaction.ApogyCommonTransactionFacade; |
| import org.eclipse.apogy.common.math.ApogyCommonMathFacade; |
| import org.eclipse.apogy.common.math.Tuple3d; |
| import org.eclipse.apogy.core.ApogyCorePackage; |
| import org.eclipse.apogy.core.Positioned; |
| |
| public class DistanceFilterCustomImpl<T extends Positioned> extends DistanceFilterImpl<T> { |
| public Tuple3d getPosition() { |
| Tuple3d tmp = super.getPosition(); |
| if (tmp == null) { |
| tmp = ApogyCommonMathFacade.INSTANCE.createTuple3d(0, 0, 0); |
| ApogyCommonTransactionFacade.INSTANCE.basicSet(this, ApogyCorePackage.Literals.DISTANCE_FILTER__POSITION, |
| tmp); |
| } |
| return tmp; |
| } |
| |
| @Override |
| public boolean matches(Positioned object) { |
| Point3d point = new Point3d(getPosition().asTuple3d()); |
| |
| Matrix4d m1 = object.getPose().asMatrix4d(); |
| Vector3d p1 = new Vector3d(); |
| m1.get(p1); |
| |
| double distance1 = point.distance(new Point3d(p1)); |
| |
| if (isInclusive()) { |
| return distance1 <= Math.abs(getMaximumDistance()); |
| } else { |
| return distance1 < Math.abs(getMaximumDistance()); |
| } |
| } |
| |
| } // DistanceFilterImpl |
